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MODELLING.  TRAJECTORY  CALCULATION  AND  SERVOING 
OF  A  COMPUTER  CONTROLLED  ARM 

by 

Richard  Paul 


The  problem  of  computer  control  of  an  arm  is  divided  into  four  parts:  modelling, 
trajectory  calculation,  servoing  and  control. 

In  mode  Ming  we  use  a  symbolic  data  structure  to  represent  objects  in  the 
envirjnment.  The  program  considers  how  the  hand  may  be  positioned  to  grasp 
these  objects  and  plans  how  to  turn  and  position  them  in  order  to  make  various 
moves.  An  arm  model  is  used  to  calc  'late  the  configuration-dependent  dynamic 
properties  of  the  arm  before  it  is  moved. 

The  arm  is  moved  along  time-coordinated  space  trajectories  in  which  velocity  and 
acceleration  are  controlled.  Trajectories  are  calculated  for  motions  along 
defined  space  curves,  as  in  turning  a  crank;  in  such  trajectories  various  joints 
must  be  free  due  to  external  motion  constraints. 

The  arm  is  servoed  by  a  small  computer.  No  analog  servo  is  used.  The  servo  is 
compensated  for  gravity  loading  and  for  configuration-dependent  dynamic 
properties  of  the  arm. 

In  order  to  control  the  arm.  a  planning  program  interprets  symbolic  arm  control 
instructions  and  generates  a  plan  consisting  of  arm  motions  and  hand  actions. 

The  move  planning  program  has  worked  successfully  in  the  manipulation  of  plane 
faced  objects.  Complex  motions,  such  as  locating  a  bolt  and  screuing  a  nut  onto 
it,  have  also  been  performed. 
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SECTION  1 
INTRODUCTION 


1.1  SYSTEM  DESCRIPTION 

Ue  are  concerned  here  with  a  computer  controlled  arm  and  hand.  This  am.  and 
hand  function  together  as  a  general  purpose  manipulator  which  forms  part  of  a 
robot  as  an  effector  subsystem. 

The  computer  plans  and  executes  sequences  of  arm  and  hand  motions  to  accomplish 
tasks.  Arm  motions  are  along  smooth,  collision  free  space  trajectories;  all 
known  forces  are  predicted  during  planning  and  compensated  for  durinti  execution. 
Hand  motions,  which  consist  of  opening  ana  closing  the  hand,  are  controlled  by 
touch  sensors.  The  hand  can  al6o  exert  a  force  while  following  an  external  lu 
defined  motion.  * 


Any  robot  must  contain  both  a  sensory  input  mechanism,  whereby  it  can  gain 
information  about  the  environment,  and  an  effector  subsystem  by  which  it  can 
change  the  environment.  In  addition  to  being  the  main  effector,  the  hand  also 
has  primitive  touch  sensors  and  force  detection  ability,  and  mau  be  considered  a 
sensory  subsystem. 


The  environment  in  which  the  robot  works  must  be  one  which  it  can  represent 
internally.  1 1  has  certain  information  a  priori,  and  is  able  to  gain  more 
information  by  interacting  with  its  environment. 


In  our  case  it  knows,  a  priori,  that  objects  are  plane-bounded,  solid,  and  may 
be  colored.  It  knows,  a  priori,  that  all  objects  are  supported  by  a  plane,  or  by 
encn  other,  and  that  objects  may  be  moved,  but  not  through  each  other.  It  is 
given  prototypes  of  all  the  possible  objects  that  can  exist,  and  learns  by  means 
of  its  senses  of  tne  existence  and  position  of  instances  of  these  prototypes. 
Uork  is  m  progress  to  increase  the  scope  of  the  robot's  environment;  curved 
objects  are  currently  being  added  lAgin], 


The  type  of  interaction  between  robot  subsystems  is  important  as  subsystems 
function  together  to  accomplish  tasks.  Currently  all  subsystems  perform  under 
the  direction  of  the  strategy  subsystem,  with  little  interaction  between  other 
subsystems.  For  instance,  the  arm  does  not  cal!  for  vision  to  locate  an  object 
tnat  it  has  dropped,  nor  does  vision  call  the  arm  to  move  an  obscuring  object  in 
order  that  it  may  see"  better.  One  important  exception  is  the  case  of  visual 
feedback  used  to  position  the  hand  on  an  object;  here  the  vision  subsystem 
interacts  d  rectly  with  the  arm  (Gill].  u 


To  illustrate  the  system  we  will  describe  the  interaction  needed  to  solve  the 
Instant  Insanity  puzzle  (Feldman  71b] .  Here  i t  is  first  required  that  four 
colored  cubes  be  found  and  the  color  of  the  faces  be  determined.  The  cubes  are 
then  turned  and  stacked  so  that  each  side  of  the  stack  has  four  di  f  ferent 
colored  faces  visible. 
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The  interaction  between  the  STRATEGY  subsystem,  VISION,  RECOGNIZE,  COLOR  and  the 
ARM  is  on  the  following  level,  The  STRATEGY  subsystem  asks  the  VISION  subsystem 
to  find  an  outline;  it  then  asks  RECOGNIZE  to  identify  the  outline  as  a  cube. 
This  process  is  repeated  until  four  cubes  have  been  found.  COLOR  is  then  told 
* o  find  the  color  at  the  center  of  each  of  the  three  visible  faces  of  each  cube. 
The  ARM  is  told  to  turn  over  each  cube  and  VISION  called  to  refind  the  outline. 
RECOGNIZE  is  then  called  to  reidentify  the  outline  as  a  cube.  Uhen  all  four 
cubes  have  been  turned  over  and  re-found,  COLOR  is  told  to  find  the  color  of 
each  of  the  three  now  visible  back  faces  of  each  cube.  The  STRATEGY  subsystem 
then  determines  the  necessary  turns  and  stack  positions  of  each  of  the  cubes  to 
obtain  a  solution.  The  ARM  is  then  told  to  move  each  cube  accordingly.  There 
are  two  levels  of  error  recovery;  within  each  subsystem,  and  by  the  strategy 
subsystem  when  a  subsystem  reports  faiure. 

The^programmi ng  environment  in  which  the  robot  operates  is  known  as  the  "Hand 
Eye"  system  [Feldman  71al.  Here  individual  subsystems  are  represented  by  time 
sharing  jobs,  which  may  interact.  This  i6  done  by  two  means:  1)  a  global  data 
structure  which  represents  information  about  the  environment  available  to  all 
subsystems;  2)  the  message  procedure  construct  whereby  one  job  may  execute  a 
procedure  in  another  job.  For  example,  cubes  are  moved  by  a  message  procedure 
"rTOVc_INSTANCE"  which,  in  the  Instant  Insanity  case,  was  used  both  to  turn  the 
cubes  over  and  to  stack  them. 

The  arm  subsystem  provides  a  series  of  functions  which  are  of  general  utility, 
such  as  to  move  the  arm  to  a  given  position  "MOVE, "  or  to  open  the  hand  "0PE1.." 
In  order  to  perform  these  functions  the  arm  subsystem  maintains  a  data  base  in 
the  form  of  constants  and  procedures  which  describe  the  arm  and  hand  in  detail. 
If  the  arm  is  required  to  perform  some  task  then  an  attempt  is  made  to  describe 
the  task  in  terms  of  existing  arm  functions,  if  this  can  be  dona  then  the 
problem  is  solved.  If  a  task  is  proposed  that  cannot  be  solved  in  terms  of 
existing  functions,  then  either  a  function  is  modified  or  a  new  function  must  be 
w  i  t ten. 

Other  subsystems  are  not  expected  to  use  the  arm's  data  base, which  is  local  to 
the  arm  subsystem.  Problem  solving  requiring  use  of  this  data  base  would  be 
considered  a  function  cf  the  arm  subsystem.  Global  variables  are  maintained 
wr,ch  describe  the  current  state  of  the  arm  for  the  other  subsystems. 

To  bu i  1  a  a  stack  of  blocks  would  not  be  an  arm  function,  as  it  could  be 
acco"’p 1  i  shed  in  terms  of  simpler,  existing  functions  such  as  MOVEJNSTANCE.  To 
turn  a  cube  over  would  be  an  arm  function  as  this  requires  that  the  range 
throughout  which  the  hand  can  grasp  the  cube  be  considered.  To  move  the  arm 
through  a  cluttered  space  of  objects  is  an  arm  task  as  this  requires  that  the 
arm  be  considered  in  relation  to  the  other  objects  when  planning  the  trajectory 
that  tne  arm  will  fcl low. 

Information  is  given  to  the  arm  subsystem  in  function  calls  and  in  the  form  of  a 
three  dimensional  description  of  the  space.  If  this  space  is  completely 
described  and  the  arm  fails  to  accomplish  a  task  then  the  strategy  subsystem 
knows  that  the  task  must  be  specified  differently.  For  instance,  if  a  block  must 


be  moved  but  some  obstacle  is  in  the  uay,  then  the  strategy  program  must  first 
ask  the  arm  to  move  the  obstruction  and  then  ask  that  the  original  move  be 
accomplished.  The  arm  subsystem  would  not  move  other  objects  and  thus  change  the 
state  of  the  environment  uithout  being  told  to  do  so  by  the  strategy  subsystem. 
!f  the  space  model  is  incomplete  and  the  arm  cannot  accomplish  a  task  based  on 
the  available  information,  then  the  arm  u:l!  not  call  the  vision  subsystem  to 
have  this  space  investigated  but  will  report  failure. 

There  are  two  main  parts  to  the  arm  subsystem:  the  planning  program  and  the  arm 
execution  program.  In  order  to  move  the  hand  a  plan  is  made.  If  the  hand  is  to 
move  an  object  then  the  planning  program  considers  the  object  in  relation  to  the 
hand  to  determine  how  it  may  be  grasped.  As  the  move  is  planned,  other  objects 
are  considered  in  relation  to  the  hand  in  order  to  prevent  collisions.  The  plan 
is  represented  in  terms  cf  a  coordinated  time  dependent  trajectory  for  each 
joint  of  the  arm.  Since  the  computer  has  planned  a  trajectory  the  program  knous 
before  the  arm  is  moved  the  configuration,  velocity  and  acceleration  of  all  the 
links  of  the  arm  and  can  compute  the  effective  inertia  and  the  gravity  torque  of 
each  link.  These  terms  together  with  the  trajectory  are  given  to  the  arm  servo 
program,  where  the  inertia  and  gravity  terms  are  used  to  improve  the  execution 
of  the  trajectory. 

Tr,e  arm  servo  program  executes  the  trajectory  by  moving  the  arm;  it  also 
performs  such  actions  as  opening  and  closing  the  hand,  Trajectories,  together 
with  hand  actions,  are  written  out  in  a  file,  and  may  be  repeatedly  executed  by 
the  arm  servo  program  if  required.  The  arm  servo  program  is  small  and  is 
suitable  for  execution  in  a  mini -computer  connected  directly  to  the  arm.  The 
planning  program  can  be  run  under  time  sharing  and  can  make  plans  for  many  such 
arms. 

The  servo  is  a  conventional  sampled  data  servo  executed  by  the  computer  with  the 
following  modification:  certain  control  constants,  the  loop  gain,  predicted 
nravity  and  external  torques  are  varied  with  arm  configuration. 

In  addition  to  the  needs  of  the  current  vision  and  strategy  subsystems,  the  arm 
n  '  been  n»*ogramrieri  to  perform  other  tasks  such  as  turning  cranks,  screwing  in 
screws,  pushing  and  pulling.  The  touch  sense  is  used  in  some  of  these  tasks.  Arm 
programs  may  be  written  where  the  course  of  execution  of  the  program  by  the  arm 
maw  be  modified  depending  on  activation  of  the  touch  sensors  or  other 
cord',  ‘ions.  Such  programs  may  be  written  in  a  form  of  assembly  language  but  are 
identical  to  the  message  procedure  calls  of  a  strategy  program. 

In  subsequent  sections  of  this  thesis  we  first  describe  the  model  of  the  arm  and 
derive  all  the  relations  tnat  we  will  use  (Section  2).  Ue  then  describe  the 
mode  of  the  environment  and  the  hand's  interaction  with  it  (Section  3). 
Section  U  describes  hOVE JfJSTANCE  the  highest  level  strategy  function  of  the 
arm  subsystem,  provided  primarily  for  strategy  subsystems  performing  operations 
with  plane  faced  solids.  In  Section  5  we  describe  the  requirements  and  solution 
of  the  smooth  trajectories  used  by  the  arm.  The  servo  loop  is  described  and  we 
then  deal  with  control  (5ection  7),  giving  a  list  of  the  arm  functions  or 
pr  i  r.i  i  t  i  ves. 
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The  arm  is  described  fui  ly  in  [Scheinmanl ,  and  we  give  a  brief  description  in 
Appendix  A.l.  Two  other  appendices  are  given.  Appendix  A. 2  describes  SAIL,  a 
form  of  ALGOL,  with  LEAP  added.  SAIL  is  the  language  in  which  the  programs  are 
written  and  we  will  use  it  to  describe  some  of  the  algorithms.  Appendix  A. 3 
briefly  describes  homogeneous  coordinate  vectors  ar.d  transformations,  more  fully 
covered  in  [Roberts  651. 


The  notation  used  in  this  work  is  as  follows:  vectors  are  represented  by  an 
underbar  V*  Matrices  are  represented  by  vertical  bars  |  M  | ;  Exponents  are 

2 


either  superscripted  or  preceeded  by  an  "t",  x 
subscripted  or  enclosed  in  square  brackets,  A 


represented  by  an  asterisk 


‘  i 


or  xt2:  Indices  are  either 
or  A t i , j ] ;  Multiplication  is 


1.2  HISTORY 

The  first  mechanical  hands  were  developed  at  Argonne  National  Laboratary  in  1947 
for  handling  radioactive  materials  [Goertz  64],  These  hands  were  master-slave 
systems  where  the  hand  replicated  the  motions  of  a  person,  the  master.  In  1948 
force  feedback  was  added  to  enable  the  operator  to  feel  the  forces  that  the  hand 
was  exerting  [Goertz  521. 

The  early  type  of  hand,  without  force  feedback,  has  been  adapted  to  perform 
repetitive  tasks  [Lindboml.  The  hand  is  moved  from  one  position  to  another  by  an 
operator  and  the  joint  positions  recorded.  The  hand  can  then  cycle  repeatedly 
through  these  recorded  positions  in  synchronism  with  external  machinery. 

In  19G1  Ernst  [Ernst!  developed  a  computer  controlled  hand  with  touch  sense.  The 
r  .j  - :  could  explore  a  region  by  touch  and  put  the  objects  it  identified  into  a 
box.  In  19G3  a  half  tone  picture  could  be  analyzed  to  locate  and  identify  plane 
faced  objects  [Roberts  S3).  By  1368  a  program  using  a  TV  camera  as  vision  input 
[Uichmanl  located  the  objects  for  the  hand  to  pick  up. 

In  1368  P leper  studied  the  kinematics  of  arms  and  planned  collision  free 
trajectories  through  spaces  containing  obstacles  [Pieperl.  This  work  uas 
followed  by  Kahn  who  studied  the  dynamics  and  developed  a  bang-bang  servo 

[Kahn] . 

By  adding  force  feedback  Inoue  was  able  to  perform  such  tasks  as  putting  a  peg 
into  a  hole  and  turning  a  crank  [Inoue] .  Ejiri  developed  a  system  to  assemble 
blocks  using  a  drawing  of  the  required  assembly  as  a  visual  input  [Ejiri],  In 
1972  Goto  could  locate  and  identify  plane  faced  objects  by  touch  and  then  pack 
them  compactly  by  moving  and  pushing  them  [Goto!. 

The  recent  proceedings  of  The  Second  International  Symposium  On  Industrial 
Robots  CIITRI3  provide  a  general  review  of  the  state  of  the  art. 


SECTION  2 


ARfl  MODEL 

In  this  section  ue  will  consider  the  arm  and  develop  a  model  for  it  [Pieper], 
Ue  will  describe  the  solution  which,  given  a  hand  position,  returns  a  set  of 
joint  angles.  Then,  based  on  the  model  of  the  arm,  we  will  develop  a  solution 
for  differential  motion.  Ue  will  then  derive  the  relation  between  acceleration 
and  force  for  the  arm,  to  obtain  the  effective  link  inertia  and  gravity  loading 
[Kahn],  Finally  we  will  derive  the  relationship  between  a  force  and  moment 
acting  at  the  hand  and  the  six  joint  reaction  torques.  The  results  of  this 
section  are  used  in  later  sections  of  the  work  but  as  they  all  relate  to  the  arm 
model  they  are  derived  together  here. 


2.1  KINEMATICS 

The  arm  shown  in  Figure  2.1  is  a  six  degree  of  freedom  device  allowing  the  hand 
to  be  positioned  anywhere  and  with  any  orientation  within  the  limits  of  joint 
motion. 

The  arm  is  made  up  of  six  links,  each  connected  to  the  next  by  a  joint.  There 
are  two  kinds  of  joints,  prismatic,  or  sliding,  and  revolute.  In  order  to 
describe  the  link  transformation  in  terms  of  the  joint  constraint  and  the  joint 
variable  we  will  introduce  a  coordinate  system  in  which  the  joint  constraint  is 
implicit. 

Ue  will  describe  the  "A"  matrices,  which  relate  between  link  coordinate  systems, 
and  the  "T"  matrices,  the  link  transformations  which  specify  the  position  and 
orientation  of  each  link  in  6pace. 

Associated  with  each  link  is  an  orthogonal  coordinate  system  fixed  in  the  link 
(see  Figure  2.2) . 

For  link  i  the  Zi  axis  is  directed  along  the  axis  of  the  joint  between  link  i 
and  i+1.  The  xi  axis  is  along  the  common  normal  between  the  two  joint  axes  of 
the  link  in  the  direction  from  z(i-l)  to  zi.  The  y  axis  completes  the  right 
handed  set. 

Ue  can  transform  coordinate  systems  i  into  i+1  by  performing  a  rotation,  two 
translations,  and  a  final  rotation  as  follows: 

1) .  A  rotation  about  zi  of  8i  to  make  xi  parallel  to  x(i+l). 

2) .  A  translation  si  along  zi  to  locate  the  origin  at  the  point  where 
the  common  normal  between  zi  and  zi+1  cuts  zi. 

3) .  A  translation  of  ai  along  x(i+l)  to  bring  the  origins  into 

coincidence. 


immst 
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Figure  2.2 

Link  Coordinate  System 


A).  A  rotation  about  x(i+l)  of 

The  j:iint  variable  for  a  revolute  joint 
jo i nt  i s  s. 


ai  to  bring  the  z  axes  into  coincidence, 
is  0;  the  joint  variable  for  a  prismatic 


In  the  case  of  the  arm  that  ue  are  using,  ue  pick  the  origin  of 
tre  base  of  the  shoulder.  Ue  have  tuo  revolute  joints,  followed 
joint,  foMowed  by  three  intersecting  revolute  joints.  There  are 
SI  »  16. 24 in,  S2  *  6.05in  and  S6  -  10.35in.  S4  -  S5  -  0,  and  S3 
The  link  coordinates  for  the  arm  are  shown  in  Figure  2.3. 


coordinates  at 
by  a  prismatic 
three  offsets: 
is  a  variable. 


If  we  express  points  in  I  ink  i  by  a  vector  Rj.  then  the 
coordinate  systems  Rj.  and  R ( i -1 )  may  be  expressed  by: 


relationship  between 


BILdi  »  |  A  i  |  *  j  R  i  | 


fEq.  2.13 


where  |  Ai  |  is  given  by: 


|  cos  8 
I  s  i  n  0 

I  0 
I  2 


-cos  a  sin  6 
cos  a  cos  0 
sin  a 

0 


sin  a 
-sin  a 
cos  a 
0 


s in  0 
cos  0 


a  cos  9  | 
a  sin  0  j 


CEq.  2.23 
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For  the  joint  angles  shown  in  the  following  table: 

Table  2.1 
Joint  Angles 

JOINT  VARIABLE 

1  -S5.7 

2  -112.4 

3  22.2 

4  -38.2 

5  83.4 

6  68.9 

The  A  matrices  for  the  arm  shown  in  Figure  2.1  are: 


1  -.13 

j 

.00 

1.00 

.00 

A1  - 

1  -1.33 

! 

.00 

-.10 

.00 

1  .03 

j 

-1.03 

.00 

16.24 

1  .03 

.08 

.00 

1.00 

1  -.38 
j 

.00 

-.92 

.00 

■ 

CM 

1  -.92 

i 

.00 

.38 

.00 

1  .00 

| 

1.00 

.00 

6.85 

I  .00 

.00 

.00 

1.00 

1  .03 

j 

1.00 

.00 

.00 

A3  ■ 

1  -1.00 

1 

.00 

.00 

.00 

.00 

j 

.33 

1.00 

22.16 

1  .00 

.00 

.00 

1.00 

1  .79 

| 

.00 

.62 

.00 

A4  - 

1  -.62 

1 

.00 

.79 

.00 

1  .03 

j 

-1.00 

.00 

.00 

1  .00 

.00 

.00 

1.00  ] 
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A5 


.17 

.00 

.99 

.00  | 

.39 

.00 

-.17 

| 

.00  1 

.00 

1.00 

.00 

j 

.00  | 

.00 

.00 

.00 

| 

1.00  | 

.36 

-.93 

.00 

.00  | 

A6 


.93 

.  36 

.00 

.00  j 

.00 

.00 

1.00 

j 

10.35  i 

.00 

.00 

.00 

| 

1.00  | 

if  we  let  link  0  be  the  table  coordinate  system  then  we  may  relate  from  anu  link 
coordinates  to  Rfi  by:  * 


Rfl  -|A1|*|A2|*|A3|  ...  |Ai |*|Ri |  tEq.  2.3] 


c  r : 


Bfl  -  I  Til  *  I  Pi| 


IEq.  2.4] 


uhere; 

I  Ti|  -  |A1|*|A2|*|A3|  ...  | A i |  [£q.  2.5] 


|  Ti  |  is  the  transform  of  the  i th  link  of  the  arm  which  describes  the  position 
of  the  link  in  table  coordinates. 

For  the  A  matrices  given  in  the  preceding  example  we  have  the  corresponding  T 
ma t  r i cc  9  s 


Ti 


-.10 

.00 

1.00 

.00 

-1.00 

.00 

-.10 

.00 

.00 

-1.00 

.00  16.24 

.00 

.00 

.00 

1.00 
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T3  - 


T4  - 


T5  - 


TG  - 


KINEMATICS 


.04 

1.00 

.09 

6.02 

.38 

-.10 

.92 

-.60 

.82 

.00 

-.38 

16.24 

.00 

.00 

.00 

1.00 

-1.00 

.04 

.09 

8.96 

.10 

.38 

.92 

19.78 

.00 

.92 

-.38 

7.79 

.00 

.00 

.00 

1.00 

-.81 

-.09 

-.59 

8.06 

-.16 

-.92 

.36 

19.78 

-.57 

.38 

.73 

7.79 

.00 

.00 

.00 

1.00 

-.23 

-.59 

-.78 

8.06 

-.93 

.36 

-,00 

19.78 

.28 

.73 

-.63 

7.79 

.00 

.00 

.00 

1.00 

-.63 

-.00 

-.78 

-.00 

.00 

1.00 

-.00 

19.78 

.78 

-.00 

-.63 

1.30 

.00 

.00 

.00 

1.00 

|  TG  |  is  the  transformation  of  the  hand,  the  1 36 1  link 
interpret  this  matrix  as  follows:  the  right  hand  column 


of  the  arm.  Ue  can 
of  the  matrix  is  the 


position,  in  table  coordinates,  of  a  point  centrally  located  between  t!,e  finger 
tips  The  second  column  specifies  the  direction  of  the  "y"  axis  of  *ne  hand 


(see  Figure  2.3),  which  we  will  call  the  orientation  vector  Q;  the  orientation 
vector  is  directed  between  the  finger  tips.  The  third  column  is  the  "z"  axis  arid 
is  in  the  direction  that  the  hand  is  pointing;  ue  will  call  this  vectoi  the 
approach  vector  Ue  may  then  write  |  TG  |  as: 
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(0  X  A)  [x] 

0  Ixl 

A  [>■] 

PIx] 

(0  x  A)  [y] 

0  [y] 

A  [y] 

Ply] 

(0  x  A)  IzJ 

0  [z] 

Atz] 

P[z] 

0 

0 

0 

1 

IEq.  2.6] 

The  approach  vector  can  be  expressed  in  terms  of  an  approach  angle  as  follows: 

Ue  define  a  reference  approach  vector  as: 

HA  -  Q  x  k  CEq.  2.7] 

where  k  is  a  unit  2  vector.  The  approach  angle  is  the  angle  between  A  and  PA 
measured  about  Q  (see  Figure  2.4). 


2.2  ARM  SOLUTION 

The  c'm  solution  is  a  procedure  which,  given  |  T6  | ,  the  transformation  of  the 
hand,  returns  the  six  joint  angles  which  will  position  the  arm  in  such  a  wau 
that  the  hand  will  have  the  required  transformation. 

Because  the  last  three  joints  intersect  we  can  obtain  a  closed  form  solution 
IP  leper].  The  position  of  the  end  of  link  3  is  found  as: 

L2  -  P  -  LS.  [Eq.  2.8] 

wr,e-e  Li  is  a  vector  the  length  of  S6  and  in  the  direction  of  the  approach 
vector  A  (see  Figure  2.5). 

A  vector  from  the  shoulder  to  the  end  of  link  3  is: 

a-LS-Ll  IEq.  2.9] 

Ue  can  now  solve  for  S3,  the  prismatic  joint  variable,  as: 

S3  -  (  U  .  U  -  S2f2  ) T  (1/2)  [Eq.  2.10] 


Having  solved  for  S3,  01  is  given  by: 

81-0  +  (|) 


Uhere  0  and  Q)  are: 


IEq.  2.11] 
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See  Figure  2.5 


no 


then; 


Figure  2.5 

Solution  of  Joints  1,2  and  3 
Sin  0  -  S2  /  (  UtlM2  +  U C23 12  > t (1/2) 


[Eq.  2.131 


°os  02  «  U[31  /  S3  [Eq.  2.141 

See  Figure  2.5. 

The  '.'nit  vectors  y3  and  z3  are  now  calculated  for  link  3  and  a  vector j 

BS  ■  z2  x  [Eq.  2.151 

is  calculated. 

Then: 

i 

94  angle  betueen  gfi  and  about  jg  [Eq.  2.161 

05  angle  between  and  z2  about  Rg.  [Eq!  2!  171 

0G  angle  between  and  gg  about  [Eq.  2.181 


See  Figure  2.6 


Figure  2.4 
Hand  Coordinates 
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Figure  2.S 

Solution  of  Joints  4,5  and  B 

All  joints  with  the  exception  of  joint  6  have  only  a  partial  range  of  motion.  As 
the  solution  for  each  joint  is  obtained,  it  must  be  checked  to  see  that  it  is 
within  the  range  of  motion  of  that  joint. 

2.3  DIFFERENTIAL  SOLUTION 

Given  ar.  arm  solution,  it  is  often  necessary  to  compute  the  differential  chanae 
the J current  ^ortentat^en^f ^tht^hand.  S"a"  ^  P°!i"0n  “hi  " 

Ue  can  obtain  the  differential  change  in  position  £|R2  with  respect  to  a  chanae 
m  joint  variable  dqj  as:  cnange 


clRfi  -  E  |Ui  j|  «  dqj  *  |  R  i  | 

j-1 


tEq.  2.19] 


uhere: 


|Uij|  -  ajTij/c'.qj 


IEq.  2.20] 
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From  which  we  obtains 


|Uij|  -  |A1|*|A2|  ...  |Aj-l |*|Qj |*|A j i*|A j+1 |  ...  *JAi| 


[Eq.  2.211 


and  depending  on  whether  the  joint  is  rotary: 

|0-100| 
0(9)  -  i  i  0  0  e  i 
100001 
100001 


[Eq.  2.22] 


or  prismatic: 

10  0  0  0  1 

Q(s)  -  |  0  0  0  0  1 

|0  0  0  1| 

10  0  0  0  1 

[Eq.  2.23] 


If  we  then  evaluate  the  six  1  UGjl  matrices  (Equation  2.20)  we  will  have  the 
differential  change  in  the  16  elements  of  the  hand  matrix  )  TB  1  for  each  of  the 
six  joints.  This  corresponds  to  16  equations  in  B  unknowns  of  which  only  6 
equations  are  independent.  Ue  must  pick  S  indeDendent  equations  of  the  IB  and 
then  solve  for  the  required  change  in  the  qj. 


From  |  UBj  1  we  pick  the  first  three  elements  of  the  right  hand  column  as  these 
vwrre spend  to  dx,  dy,  dz.  Ue  then  pic*  the  two  smallest  elements  of  column  3, 
the  approach  vector,  to  constrain  its  direction.  Finally  we  pick  one  add  i  tional 
element  from  column  2.  the  orientation  vector,  in  order  to  constrain  rotation 
about  the  approach  vector. 


The  six  equations: 


.  >wt  i*.  .  .  ssJtMSdMSb 
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dx  | 

|  U61 

U62 

UG3 

UG4 

UG5 

UGG  | 

1  dq  | 

1 

1 

1  14 

1 

14 

14 

14 

14 

14  | 

I  1 1 

1 

dy  | 

|  US1 

UG2 

UG3 

U54 

U65 

UGG  | 

1  1 

1  dq  | 

1 

!  24 

1 

24 

24 

24 

24 

24  | 

| 

1  2| 

9  1 

dz  | 

■  |  UG1 

UG2 

UG3 

U64 

UG5 

UGG  | 

1  1 
*  j  dq  j 

1 

I 

1  34 

34 

34 

34 

34 

34  | 

j 

1  3| 

1  1 

1 

a  i 

|  U61 

UG2 

U53 

UG4 

UG5 

UGG 

1  1 

I  dq  j 

l 

i 

u3 

u3 

u3 

u3 

u3 

u3  | 

1  3| 

I  i 

I 

i 

0  j 

I  U61 

U62 

U63 

UG4 

UG5 

UGG  | 

1  1 

1  dq  j 

1 

i 

1  v3 

v3 

v3 

v3 

v3 

v3  | 

j 

1  4| 

1 

0  j 

|  UG1 

UG2 

UG3 

UG4 

UG5 

UGG 

1  1 

1  dq  j 

1 

1  u2 

w2 

u2 

m2 

w2 

w2  j 

1  6| 

fEq.  2.24] 

are  then  solved  to  give  the  six  dqj,  the  differential  change  in  joint  angle. 

For  the  position  that  we  have  been  considering  and  have  defined  the  A  and  T 
matrices.  A  differential  change  of  dz  =  3.2  in  is  found  to  be: 

Table  2.2 

Differential  Change  of  Joint  Angle 
JOINT  dq 

1  0.0 

2  7.2 

3  -1.1 

4  -1.0 

5  4.4 

S  5.7 


2.4  DYNAMICS 

From  the  kinematic  arm  ;:.oriel  we  can  also  develop  the  dynamic  model  [Uicker],  Ue 
will  derive  the  Lagrangian  [Kahn]  for  the  arm  in  a  gravitational  force  field, 
and  obtain  the  equations  relating  acceleration  to  joint  torque,  including  the 
static  torques  necessary  to  overcome  the  effects  of  gravity.  These  results  will 
be  used  in  the  section  which  relates  to  servoing  the  arm. 
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If  the  Lagrangian  L  is  defined  as: 


L  -  K  -  P 


[Eq.  2. 25] 


where: 

K  is  the  Kinetic  Energy  of  the  system  in  terms  of  joint  variables? 

P  is  the  potential  energy  in  terms  of  joint  variables. 

The  joint  variables  "q"  are  either  "0"  or  "s"  depending  upon  whether  the 
joint  is  revolute  or  prismatic. 

The  equations  of  motion  are  given  by: 


d/dt  (  3L/<3qj  )  -  3L/9qj  -  Fj  [Eq.  2.26] 

for  j  w  1,2,  ...  n 
where  Fj  is  the  force  on  joint  j. 

From  Equation  2.19  ue  can  obtain  the  velocity  of  any  point  as: 

dR2/dt  -  |  Vi  |  *  '  Ri  |  [£q.  2.27] 


where: 


I  Vi  | 


■  I  (  |  Ui j  |  *  dqj/dt  ) 

W 


[Eq.  2.28] 


Ue  may  now  express  the  kinetic  energy  of  a  link  as  follows.  Consider  a  particle 
of  mass  dm  on  link  i  at  01  then  the  kinetic  energy  is: 


•  • 

dHi  -  1/2  (  Si  •  Bi  )  dm 


[Eq.  2.29] 


or: 


T  T 

dHi  *  1/2  Trace ( |  Vi |*|  Ri|*|  Ri|  #|  Vi j  )*qm 


[Eq.  2.30] 


The  total  kinetic  energy  for  the  link  can  be  found  by  integrating  over  the  mass 
of  the  I i nk. 
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T  T 

dHi  ■=  i/2  Trace 1 1 V i  |#(/  |Ri|*|Rij  *dm)*|Vi|  ] 

link 


to  obtain: 


Hi  =  mi* 


2  2  2  2 

1/2 (~k  +k  +k  )  k 

ill  i 22  i 33  i 12 

2  2  2  2 

k  k  -k  +k 

i 12  ill  i 22  i 33 


2  2  2 
k  +k  -k 
ill  i 22  i 33 


IEq.  2.3i: 


IEq.  2.321 


where: k  is  the  radius  of  gyration  of  link  i  about  the  i.k  axes.  xi.  ui  zi 
ijk  a 

is  the  center  of  mass  of  link  i.  mi  is  the  mass  of  link  i. 


N'netic  energy  of  the  entire  system  is: 


n  T 

K  -  1/2  E  Trace(|  Vi  |*|  Hi  |*|  Vi  |  ) 
i  »1 


IEq.  2.33] 


The  potential  energy  of  the  system  due  to  gravity  in  the  negative  z  direction  is 
expressed  by: 


P  -  -  I  mi  *  |  G  |  *  |  Ti  |  *  |  Ri  | 
i«l 


where:  |  G  |  ■  |  0  0  g  0  j 

and  g  is  the  acceleration  due  to  gravity. 


IEq.  2.34] 


IEq.  2.35] 
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Substituting  for  <  from  Equation  2.33  and  for  P  from  Equation  2.34  into  Equat 
Z.-'b  and  then  differentiating  according  to  Equation  2.2G  ue  obtain 


n 

Fi  -  Z 
j-' 


j  T  .. 

I  Trace(|  Ujk|*|  Hj|*l  U j i |  *qk) 

k-1 


n  j  j  j 

+2  Z  Z  Iracetl  Ujkp|*|  Hj|*|  Uji|  *qj*qk) 
j-i  k-1  p-1 


n 

-I  mj*|  G  |*|  Uj i |*| 
j-i 


RJ 


CEq.  2.36J 


the  equation  relating  acceleration  to  force,  from  which  ue  will  infer  the 
effective  link  inertia  and  gravity  loading  torque  Tg  in  Subsection  6.1. 


2.5  FORCE  AND  MOMENTS 


Given  a  force  F_  that  acts  through  the  origin  of  the  hand  coordinate  system  and  a 
moment  M,  we  wish  to  find  the  joint  reaction  torques. 

Ue  represent  a  force  in  the  n‘ th  coordinate  system  as: 

I  FnCx] 


Fn 


I 

I  Fn[y] 

I 

I  Fn[zJ 

I 

I  0 


fEq.  2.37] 


and  a  moment  similarly: 


Mn 


MnLx] 
Mn  [y] 
Mntz] 


0 


CEq.  2.38J 
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f 


h®.  first  transform  the  force  F  and  the  moment  (1  into  the  hand 


coordinate  system 


i  FS  | 

I  ns  i 


-l 

|  TS  j  *|  F  | 

-1 

|  TS  |  *|  (1  | 


tEq.  2.331 
tEq.  2.40] 


Ue  then  proceed  to  transform 
fol lows  (see  Figure  2.7) : 


the  force  and  moment  back  through  the 


I  i nks  as 


and  the  moment: 


F(n-lj  |  =  j  An  |  *  |  Fn 


tEq.  2.41] 


I  Fl(n-l)  i  =  |  An  |  *  J  fin  | 


However  the  force  Fin-1)  does  not 
coordinate  system  (see  Figure  2.7)  and 


act  through  the  origin  of 
we  haiy  an  additional  moment 


tEq.  2.42] 

the  (n-1 )  h 
given  by: 


P  »  F (n-1 ) 


tEq.  2.43) 


Figure  2.7 
Force  Transformat i on 


where:  P  is  tne  right  nand  column  of  |  An  J 


the  totat  moment  is  then: 

I  n ln-1 )  |  =  |  An  |  *  i  fin  j  +  P  x  F(n-l)  [Eq.  2.44} 


If  the  (n-l)th  joint  is  revolute  then  the  reaction  torque  is  M(n-l)[zl» 
prismatic  then  the  force  is  F(n-l)tz]. 


if  it  is 


The  six  reaction  torques  are  known  as  the  equivalent  arm  torque. 

Uhen  the  hand  is  to  exert  o  force  the  equivalent  arm  torque  is  calculated  and 
added  to  the  gravity  torque  for  each  joint.  If  each  joint  is  run  at  these 
torques  then  the  hand  will  exert  the  required  force. 


For  the  arm  position  we  have  been 
to  exert  a  force  of  -102oi.  in  the 
moment  o *  -100oz.  in.  about  the  z 


considering  we  have  two  examples,  the  first 
z  direction,  the  second  example  is  to  exer't 
axis: 


i  s 

a 


Table  2.3 


Equi va 

lent  Arm  Torque 

s 

F  irzj  ■ 

■I 23oi. 

nczi  -- 

100OZ.  il 

JOINT 

TORQUE 

JOINT 

TORQUE 

1 

0.0 

1 

-100.0 

2 

-1958.3 

2 

0.0 

3 

38.2 

3 

0.0 

4 

-741.3 

4 

38.2 

5 

-230. G 

5 

-72.7 

G 

0.2 

6 

G2.7 

j 

1. 

I 


-- 
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SECTION  3 
WORLD  MODEL 

This  section  describes  the  model  of  the  arm's  environment,  which  consists  of 
solid  plane-faced  objects.  These  are  the  objects  that  the  vision  system  can 
identify  [Falk]  and  a  representat  i  on  of  them  is  maintained.  This  is  done  for 
both  the  Arm  and  the  Vision  programs,  which  share  a  common  data  base  [Pauli. 

Ue  will  first  describe  the  prototype  representation  and  the  manner  of  specifying 
instances  of  these  prototypes.  The  problem  of  grasping  this  class  of  objects  is 
then  reduced  to  finding  a  set  of  orientation  vectors. 


3.1  PROTOTYPE  DESCRIPTION 

Objects  are  described  in  terms  of  prototypes.  To  identify  an  object  is  to 
associate  the  object  with  its  prototype;  one  prototype  can  represent  many 
objects  or  instances.  All  common  information  relating  to  the  instances  is  kept 
only  once,  with  the  prototype.  The  position  of  an  object  is  associated  with  the 
instance,  as  every  instance  has  a  different  position.  The  prototype  is  located 
with  its  center  of  mass  at  the  origin  and  its  principal  inertial  axes  aligned 
with  the  coordinate  axes.  Each  vertex  is  represented  by  a  vector  giving  its 
distance  front  the  origin  and  each  face  is  represented  by  a  row  matrix  giving  its 
position  and  outward  pointing  normal. 

I  tents  are  created  for  each  part  of  a  prototype  (face,  vertex,  edge).  (Readers 
not  familiar  with  "items"  should  consult  Appendix  A. 2.)  The  spatial  information 
associated  with  vertices  and  faces  is  stored  as  array  datums  of  these  items. 
Farh  of  these  items  is  associated  with  the  prototype,  which  is  itself  an  item. 
0  tferent  attributes  are  used  to  indicate  which  topological  part  is  being 
associated. 

For  example,  in  the  case  cf  the  cube  shown  in  Figure  3.1: 

FACE  ®  CUBE  s  FI  [Eq.  3.1) 

FACE  e  CUBE  s  F2 

II  ll  II 

FACE  ©  CUBE  s  F5 

VERTEX'  »  CUBE  *  VI  IEq.  3.2) 

VERTEX  ®  CUBE  a  V2 

II  It  H 

VERTEX  »  CUBE  .  V8 

EDGE  ®  CUBE  b  El  [Eq.  3.3] 

EDGE  ®  CUBE  b  E2 

ii  H  ll 

EDGE  «  CUBE  b  El 2 
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Figure  2.1 
Cube  Prototype 

In  the  case  of  edges,  the  length  of  the  edge  is  kept  as  its  datum. 
For  each  Face  ue  now  associate  its  vertices  and  edges  together 
Consider  for  example  face  FI: 

BOUNDARY  e>  FI  s  El 
BOUNDARY  ®  Fi  *  E2 
BOUNDARY  ®  FI  s  E3 
BOUNDARY  ®  FI  s  E4 

CORNER  ®  FI  *  VI 
CORNER  ©FI  h  V2 
CORNER  ©FI  s  V3 
CORNER  a  FI  >  V 4 


And  for  edges  we  associate  the  edge  with  its  endpoints: 

ENQPT  ©  El  s  VI 
ENDPT  ®  El  s  V2 

etcetera. 


IEq.  3.4] 


IEq.  3.5] 


IEq.  3.63 


Prototypes  are  kept  for  the  objects  shown  in  Figure  3.2,  this  data  is  kept  in 
the  global  data  store  and  is  available  to  all  programs* 
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WEDGE  124 


Figure  3.2 
Prototypes 


Page  26 


PROTOTYPE 


Based  on  such  a  description 
example: 


it  is  possible  to  find  many  relationships,  for 


Given  any  face  FI  find  its  neighbors  which  share  a  common 

Ue  can  find  the  faces  which  share  the  common  vertex  VI 
the  faces  which  satisfy 


vertex  VI. 
by  finding  all 


FOREACH  F  |  VERTEX  *  F  »  VI 


Here  F  represents  an  item  variable  satisfied  by  the  association.  The  set 
of  these  F  s  are  all  the  faces  which  share  this  common  vertex.  However 
they  are  not  all  neighbors  of  FI.  To  ascertain  that  they  are  neighbors 
we  must  require  that  they  share  a  common  edge  and  that  they  are  not  Fl 


FOREACH  F,E  |  VERTEX  ®  F  «  VI 
a  EDGE  ®  F  ■  E 

a  EDGE  ®  Fl  i  E 

a  F  n  Fl 


This  specifies  the  set  of  faces  F 
share  the  common  vertex  VI. 


which  are  the  neighbors  of 


Fl  and 


Another  example: 
starting  wi th  VI 


it  is  required  to  go  around  the  vertices  of  a  face  Fl  in  order 


T  -  VI; 

FOREACH  E.H  |  EDGE  •  Fl  •  E 
a  END  ®  E  ■  T 

a  END  e  E  s  H 

A  H  x  T  DO  BEGIN 

IF  H.  -  Vl  THEN  DONE: 
<statement>; 

T  *  H  END; 


When  an  instance  is  identified:  a 
the  prototype  as  fol lows: 


new  item  is  created  which  is 


associated  with 


INSTANCE  »  CUBE  i  INST1 


[Eg.  3.7] 


The  position  and  orientation  of  the  instance 
matrix  which  relates  prototype  coordinates 
Subsection  A. 3).  This  A  x  A  transformation 
instance. 


are  expressed  as  a  transformation 
to  instance  coordinates  (see 
is  stored  as  the  datum  of  the 


Most  calculations  can  be  performed  by  transforming  the 
prototype  rather  than  by  transforming  the  prototype  out  to 


instance  back 
the  instance. 


to  the 


Consider  for  example  the  problem  of  finding  the  support  face  of  a  body  by 
finding  through  uhich  face  the  weight  vector  passes.  Ue  could  transform  each 
face  out  to  the  instance,  calculate  if  the  weight  vector  was  in  the  general 
direction  of  the  outward  pointing  normal  and  then  determine  whether  the  weight 
vector  actually  passes  through  the  face.  This  would  require  that  we  transform 
each  vertex  of  the  face  to  the  instance.  It  is  more  efficient  to  transform  the 
weight  vector  back  to  the  prototype  hu  using  the  inverse  transform  and  check 
through  which  face  it  passes,  avoiding  all  the  other  transformations,  which  take 
of  the  order  of  0.5  m  sec.  each. 

Ulith  this  "prototype-instance"  scheme  we  can  represent  all  plane  faced  objects. 
There  is  sufficient  information  available  for  the  vision  program  to  be  able  to 
identify  objects  in  two  dimensional  scenes.  In  the  next  section  we  will  ehow 
that  there  is  also  sufficient  information  for  the  arm  program. 


3.2  ORIENTATION  VECTORS 

The  prototype  description  is  used  when  it  is  required  to  move  an  instance  of 
some  prototype.  Apart  from  the  positional  information,  which  is  obtainable 
directly  from  the  instance  transform,  the  prototype  description  is  used  to 
calculate  how  the  instance  may  be  picked  up.  Knowing  how  a  body  is  oriented  and 
where  it  is  located  does  not  specify  a  hand  position  which  may  be  used  to  pick 
the  instance  up.  Although  there  are  an  infinite  number  of  ways  in  which  an 
object  maybe  picked  up,  we  will  limit  the  possibi  I  i  ties  by  the  following 
heuristics.  Ue  will  require  that  the  object  be  picked  up  by  two  parallel  faces 
on  an  axis  containing  the  center  of  mass,  as  this  will  prevent  the  object  from 
rotating.  One  but  not  both  surfaces  may  be  replaced  by  an  apex  of  the  body.  Both 
surfaces  may  be  replaced  by  edges  if  a  normal  from  the  edge  intersects  the 
center  of  mass.  These  heuristics  define  a  set  of  orientation  vectors.  if  the 
hand  is  positioned  at  the  center  of  mass  uith  one  of  these  orientation  vectors, 
it  ui 1 1  be  in  a  position  to  grasp  the  object  (see  Figure  3.3). 

To  find  systematically  all  the  possible  orientation  vectors  the  program  first 
makes  a  list  of  all  the  vectors  from  the  center  of  mass  of  the  object  that  1) 
intersect  and  are  normal  to  any  edge,  2)  intersect  and  are  normal  to  any  face,  3) 
pass  through  any  apex.  Such  vectors  are  known  as  contact  vectors.  This  list  is 
then  searched  for  pairs  of  anti-parallel  vectors,  being  careful  not  to  take  both 
vectors  from  the  third  class.  This  is  done  in  the  following  manner:  To  find  the 
contact  vectors  for  faces  the  program  simply  checks  that  the  perpendicular  from 
the  center  of  mass  to  the  plane  lies  inside  the  face  and  thus  the  surface  is 
perpendicular  at  the  contact  point,  „ir: 
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ORIENTATIONS 


FORFACH  F  |  FACE®PROTOTYPE»F  DO 
BEGIN 

D-DATUN(F) [4] j 

IF  D>0.8  THEN  GO  TO  NFACE; 

Comment  if  this  plane  is  used  the  center  of  mass 
will  not  lie  between  the  finger  tips; 

IF  CONTAINED (DATUn (F) ,  F)  THEN  FOUND  ONE; 
Comment  if  the  point  of  intersection  of  the 
normal  and  the  face  within  the  boundary  then 

.iCirr  PKt  the  P0'nt  in  the  list  of  contact  vectors; 
NFACE:  END; 


The  procedure  CONTAINED  counts  the  number 
from  the  point  to  infinity.  If  the  number 
region,  if  even  then  it  is  outside. 


of  region  boundary  crossings 
i s  odd  then  the  point  i s 


of  a  ray 
inside  the 


To  find  the  contact  vectors  for  edges 
the  center  of  mass  intersect  the 
represent  an  outside  corner.  By  the 
the  first  condition: 


it  is  necessary  that  a  perpendicular  from 
edge.  It  is  also  required  that  the  edge 
following  algorithm  the  program  satisfies 


;■ 

-1 
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FOREACH  E.A.BIEOGEoPROTOTYPEsE 
a  ENOs'E»A 

a  END$E«B 

a  A*B  00  BEGIN 

T«-A  .  -  fi)/(A  -  &)  .  (&  -  B.) ; 

Comment  T  is  the  directed  distance  from 
end  point  A  to  the  normal,  divided  bg  the 
directed  distance  of  A  from  B: 

IF  T<0.0  v  T>1 . 0  THEN  GO  TO  NEOGE; 

Comment  the  normal  intersects  on  the  edge: 

£  -  A  -  ((A  -  ft)  *  T); 

Comment  C  is  a  vector  from  the  center  of 
mass  and  perpendicular  to  the  edge  at  the 
point  of  contact  (see  Figure  3.4). 

Now  check  that  this  is  an  outside  edge; 

FOREACH  N1 , N2 | BOUNDARYeNl «E 
a  B0UNDARYeN2eE 
a  Nl-Ni  DO; 

N!  -  DATUM (Nl) 5 
N2  -  0ATUI1  (N2) ; 

N  -  Ni  X  N2; 

comment  N  is  a  reference  vector  such  that 
we  move  outside  as  we  rotate  about  N  from 
Hi.  to 

V  *-  til  X  Q; 

IF  N  .  V  <  0 

Comment  £  points  to  the  outside  of 
the  vertex; 

THEN  FOUND  ONE; 

END; 

END; 

To  determine  contact  vectors  at  verticies  we  can  use  onlg  the  outside  corners  of 
the  object  (see  Figure  3.5).  That  is,  for  all  edges  at  this  vertex  the  angle  0 
must  be  less  than  90  degrees  (see  Figure  3.5). 

FOREACH  V  |  VERTEX  c  BOY  -=  V  DO  BEGIN 
C2-V  .  y. 

F0REACH~E,A  |  END  ®  E  a  V 
a  END  «  E  s  A 

a  A  *  V  DO 

IF  A  .  V  >  C2  THEN  GO  TO  N0G00D; 

FOUND  ONE; 

N0G000;  END; 


The  program  then  searches  this  list  of  contact  points  looking  for  pairs  of  anti- 
parallel  vectors,  being  careful  not  to  take  both  vectors  from  the  class  of 
vertices.  These  then  are  the  orientation  vectors  which  are  stored  with  the 
prototype  together  with  the  contact  information: 


I 

t 


Page  31 


ORIENTATIONS 


wm 


ORIENTATION  «  BDY  >  01 
ORIENTATION  •  BDY  ■  02 

"  «  ii 

ORIENTATION  ®  BDY  *  09 


(Eq.  3.81 


CONTACT  ®  01  >  FI 
CONTACT  •  01  s  F3 
CONTACT  ®  02  ■  F2 
CONTACT  •  02  »  F4 

"  IS  Ii 

CONTACT  ®  09  «  E6 
CONTACT  ®  09  ■  E2 


[Eq.  3.9] 


The  contact  information  i6  used  in  determining  which  orientation  vectors 
used  for  a  given  instance.  The  datum  of  an  orientation  vector  is  a  5 
matrix  with  the  t'oliouing  elements:  i f  &  and  £2  are  two  anti-parallel 
vectors  then  the  datum  of  the  orientation  vector  is: 


can  be 
e I ement 
contact 


Oil]  -  Cl  [II  /  {  Cimt2  +  Clt2]t2  +  Cl [33 12  )  Q/2> 

o  [23  -  Cl  [21  /  (  Cl  cm2  +  Cl  [23 12  +  Clt33t2  ) 

0131  -  Cl  [3.1  /  (  Cl  11312  +  Cl  [23 12  +  Cl  [33 12  ) 

0143  -  I  Cl  | 


In  the  case  of  a  rectangular  parallelepiped  of  size  1.25in.  x  1.25in.  x  2.55in. 
the  program  computes  the  following  9  orientation  vectors: 

Table  3.1 


at  ion  Vectors 

for  Rectangular  Paralle 

Dili 

012) 

0  C33 

0  [43 

0  [53 

1.00 

0.00 

0.00 

1.80 

-1.60 

0.00 

1.00 

0.00 

1.80 

-1.60 

8.00 

0.00 

1.00 

0.78 

-0.78 

0.71 

0.71 

0.00 

1.13 

-1.13 

0.71 

-0.71 

0.00 

1.13 

-1.13 

0.44 

0.00 

0.90 

0.70 

-0.70 

0.44 

3.00 

-0.30 

0.70 

-0.70 

0.00 

0.44 

-0.90 

0.70 

-0.70 

0.00 

0.44 

0.90 

0.70 

-0.70 

Each  rou  in  Table  3.1  represents  an  orientation  vector, 
elements  0(11,  0  C23  and  0C3I  give  the  direction  components  of 
two  contact  vectors  are  given  by: 


The  first  three 
the  vector.  The 
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£1 


I  Oil]  | 
I  I 
I  OI2J  | 
I  I 
I  Ot33| 
I  I 
I  OI43| 


£2 


I  oil] 
0123 
0133 1 
OC53 ! 


sM.sr!s.*siss!a  ixzmz 


i 
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SECTION  4 
MOVE  INSTANCE 

obje?I°CmVEJNSTANCES ifCf  irst^uireS^tVJin^  ^h™!!  t0  the  arm  move  an 

the  hand  to  grasp  the  object  located  by  I  Tj  |  I t^hn^J'Iii00  “hich  enables 
hand  position  from  which  the  hand  can  release  th*  nh-*  ♦  d  th*n  f,nd  a  8econd 
specified  by  |  Tf  |.  The  suff ixes v  anl  *r  Dbj?ct*uch  that  i 1  will  be 
positions.  sutt i xes  i  and  f  refer  to  the  initial  and  final 


4.1  RANGE  OF  SOLUTION 

initial  Pand *f  ?na°bpos U  i ons.  ^To^o  this'the  prograSn'fC°Sii?hrati0nS  3t  both  the 
approach  angles  for  each  orientatior  vector-  ?♦  thPn  th®  range  of  Po88'ble 

from  the  intersection  of  the  two  ranges!  'ci  e  alii?"  80"!e  aPProe~M  angle 
vector  the  program  needs  to  compute  ihe  ranoe  n?°  '* " °n  fd  an  ^ientat  ion 
which  the  arm  can  reach  the  object  subject  tcAhe °f  approach  angle  throughout 
arm  (see  Figure  4.1).  J  subject  to  the  physical  constraints  of  the 


A  L'MITING 

approach  angle 


Figure  4.1 

Approach  limited  by  Arm 

exist,  i,  one  exTetj”  STIurS*  tLfls  .S.” 


RANGE 


solutions  exist  for  all  approach  angles.  If  a  solution  exists,  but  not  for  the 
full  range,  the  limits  of  the  range  are  found  by  conducting  a  binary  search, 
using  the  arm  solution  procedure  to  test  for  feasibility. 

In  order  to  ensure  that  the  arm  does  not  penetrate  the  support  plane  (see  Figure 
4.2),  the  program  computes  the  range  of  approach  angle  for  which  link  5  is  above 
the  plane. 


Figure  4.2 

Approach  Angle  Limited  by  Support  Plane 


This  range  of  approach  angle  is  then  intersected  with  the  possible  range  of 
approach  angle  defined  oy  the  arm  to  give  a  range  throughout  which  the  arm  can 
reach  the  object  limited  by  both  the  arm  and  the  support. 

This  process  is  repeated  for  the  support  post  of  the  arm.  A  final  intersection 
of  ranges  is  made  to  keep  the  hand  from  intersecting  the  post,  and  the  limiting 
range  of  approach  angle  is  obtained.  Possible  conflicts  with  adjacent  objects 
are  not  considered. 


4.2  MOVE  INSTANCE 

The  first  action  of  NOVE_INSTANCE  is  to  select  those  orientation  vectors  of  the 
prototype  (  see  section  3.2)  which  can  be  used  to  grasp  the  object.  To  do  this 
the  program  transforms  a  gravity  vector  back  to  the  prototype,  using  the  inverse 
transform,  and  determines  through  which  face  it  passes.  He  consider  this  to  be 


«%*>  at*  *HUM Pet* 


the  support  face.  Any  orientation  vector  (Equation  3.8)  which  has  a  contact 
point  (Equation  3.9)  on  this  face  or  on  any  edge  or  vertex  of  this  face  is 
discarded;  the  remaining  or ientation  vectors  are  marked  as  possible.  This 
procedure  is  performed  for  the  object  in  its  initial  position  and  again  in  its 
final  position.  If  the  action  is  to  be  accomplished  in  one  move  then  the  set  of 
a^ilable  orientation  vectors  that  can  bs  used  is  the  intersection  of  the  two 
sets  of  ui  ientation  vectors  for  the  initial  and  final  positions.  A6  the  most 
stable  way  to  pick  up  in  object  is  by  grasping  the  object  by  its  faces,  the 
orientation  vectors  are  ordered  by  length  and  the  shortest,  representing  face- 
face  contacts,  are  considered  first. 

For  each  of  the  orientation  vectors  so  ordered  the  range  of  approach  is 
calculated  at  both  the  initial  and  final  positions.  In  order  to  relate  these 
two  ranges  for  a  given  orientation  vector  we  make  use  of  the  reference  approach 
vector  Equation  2.7.  By  transforming  the  reference  approach  vector  RAi  at  the 
initial  position  to  the  final  position  by: 


-1 

RAj.'  4-  |  Tf  |  *  |  Ti  |  *  |  RAi  j  (Eq.  4.1] 


the  shift  S,  between  approach  ranges  may  then  be  calculated  as: 

S  the  angle  between  gAj.1  and  BM  about  Qi 


IEq.  4.2] 


After  applying  the  shift  S  to  the  initial  range  the  two  ranges  of  approach  are 
intersected.  If  the  intersection  is  not  empty  then  the,  solution  has  been  found. 
An  approach  vector  &  is  picked  within  the  range  of  intersection.  In  order  to 
keen  the  arm  clear  of  other  obj^'ts,  the  preferred  approach  direction  is 
straight  down.  Two  arm  solutions  are  obtained,  one  at  the  initial  position  and 
one  at  the  final  position,  such  that  the  shift  between  approach  vectors  is 
maintained. 

If  there  is  no  intersection  between  the  first  two  ranges,  the  next  orientation 
vector  in  the  intersection  of  the  available  set  of  orientation  vectors  is  tried. 

When  the  set  of  orientation  vectors  is  empty  at  either  the  initial  or  final 
position,  there  is  no  way  that  the  move  can  be  accomplished  as  the  arm  cannot 
reach  the  object.  If  the  intersection  of  the  set  of  orientation  vectors  were 
empty,  or  if  after  evaluating  the  ranges  for  all  the  orientation  vectors  the 
intersection  of  the  ranges  was  empty  then  an  intermediate  position  is  tried. 

In  the  case  of  the  rectangular  parallelepiped  whose  orientation  vectors  we 
obtained  (Table  3.1)  we  will  consider  the  problem  of  moving  an  instance  from: 


1.00 

.00 

.00 

20.00 

.00 

1.00 

.00 

30.00 

.00 

.00 

1.0C 

1.30 

.00 

.00 

.00 

1.00 

-1.00 

.00 

.00 

40.00 

.00 

1.00 

.00 

20.00 

.00 

.00 

-1.00 

1.30 

.00 

.00 

.00 

1.00 

This  move  includes  turning  the  object  upside  doun. 


0f-the  pos,!'ble  orientation  vectors  the  range  of  approach  is  calcula 
at  both  positions.  PI  IS  the  position  vector  and  Qi  is  the  orientation  vector 
the  initial  position  and  at  the  second  position  the  vectors  are  £ 1  and  Q Z 


ted 

at 


El  -  20.00  30.00  1.30  1.00 

01  -  .00  1.00  .00  1.00 

Range  From  40  to  173  degrees 
Shifted  Range  From  220  to  353  degrees 


E2  -  40.00  20.00  1.30  :,00 

02  ■  .00  1.00  .00  1,00 

Range  From  7  to  120  degrees 
Common  range  0  degrees 


El  -  20.00  30.00  1.30  1.00 

01  ■  1.00  .00  .00  1.00 

Range  From  62  to  173  degrees 
Shifted  Range  From  242  to  353  degrees 

E2  -  40.00  20.00  1.30  1.00 

02  -  -1.00  .00  .00  1,00 

Range  From  7  to  123  degrees 
Common  range  0  degrees 


© 


El  -  20.00  30.03  1.30  1.00 

Qi  =  .71  -.71  .00  1>00 

Range  From  43  to  173  degrees 
Shifted  Range  From  229  to  353  degrees 

£2  -  40.00  20.00  1.30  1.00 

02  -  -.71  -.71  .00  1.00 

Range  From  23  to  155  degrees 
Common  range  0  degrees 


El  ■  20.00  30.00  1.30  1.00 

Qi  B  -7i  .71  .00  1.00 

Range  From  59  to  173  degrees 
Shifted  Range  From  239  to  353  degrees 

E2  -  40.00  22.00  1.30  1.00 

02  -  -.71  .71  .00  1.00 

Range  From  7  to  115  degrees 
Common  range  0  degrees 

As  the  intersection  of  al  I  the  ranees  is  ■’pm  an  atinMni  • 

plan  to  move  the  object  trra  i ^  tni J f al  Soli ??on  In  ^  '?  "fde  ,0  nak'  3 
and  then  from  the  interuediate  position  to  the  final  position  °0!!!'°n 

E5T  TS-  --  *  v  !;" 

siii  r::  ranse  •* 


final  position  as  the' inter.edia te p  lion  in s™  ma  ,^'eL „  ‘"'f'*'  °r 
^h.ch  to  place  the  object.  Even  hen  e  i  P  ®  15  clear  ln 

intermediate  position,  the  movers usua  N  u  accon »  i  shL  T  ' !  US6d  as  the 
position  and  not  the  orientation  that  is  specified  P  Oncost  nr  'J  •  'S  °nly  the 

eKampte^th^ob  jectrmustUbeaturned  'over6]  ■Ms*  then  * $  becau  *  ^s^n^the^presen  t 
«hee0bjec,  part-way  ovar.'pT'?  ZZ'l? 

Given  the  neui  intermediate  position  a  ept  n*  rtn.c;ini. 

determined  as  the  intersection  of  the  =et  of  f-m!  h-%  new  support  faces  is 
original  support  facs  and  of  {hi  f 7na I  LporT  a«  T  T9”?'"'5  ?'«><• 
constructed  for  this  intermediate  pos  i  t  i  onP*o  hrinS  th!  transformat  ion  is 
parallel  to  the  original  support  surface  To  ciivp  navi*  m  t  supP°rt  face 

range,  the  object  is  then  tu-ned  about  an’axis  1™  !  freecioni  of  approach 

object's  principal  a.es  perpend r £ Z, °  £u,H°r *°  "a?f  ,ha 
distance  from  the  center  of  mass  to  the  orio  nai  the  lf  the 

the  initial  distance,  the  height  of  the  cenfer  of  ?£e  HbjlcT  {'ad^s^r'  'r°m 

Ihe  process  is  then  repeated  to  tied  a  coon  approach.  First  a  aove  solution  to 
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the  intermediate  position  is  computed  and  then,  if  successful,  a  move  solution 
from  the  intermediate  position  to  the  final  position  is  attempted  .  Ranges  are 
saved  during  the  "initial  -  final"  move  attempt  as  they  are  needed  in  the 
"initial  -  intermediate,"  "intermediate  -  final"  move  evaluations. 

In  the  example  we  are  considering  the  intermediate  position  is  specified  as  30, 
30,  1.3  and  as  the  intersection  of  all  the  ranges  was  zero  an  intermediate 
pos i t ion  is  set  up  as: 

|  .00  .13  -.98  30.00  | 

I  I 

|  .03  .38  .19  30.03  | 

I  I 

!  1.03  .03  .00  .GS  | 

I  I 

.33  .30  .33  1.03  | 


The  program  now  tries  to  make  a  plan  to  move  the  object  to  this  position: 

Pi  *  23.33  30.33  1.33  1.00 

Qi  -  .03  1.33  .30  1.00 

Range  From  40  to  173  degrees 
Shifted  Range  From  -50  to  83  degrees 

Pi  -  33.03  30.33  .65  1.00 

Qi  «  .19  .98  .00  1.00 

Range  From  19  to  15G  degrees 
Common  range  From  19  to  83  degrees 
Approach  «  51  degrees 

A  common  range  exists  and  the  program  now  calculates  the  two  hand  positions: 


|  -.63 

.03 

-.78 

20.03 

.30 

| 

1.83 

.00 

33.33 

1 

.78 

.00 

-.  o3 

1.33 

j 

|  .03 

.03 

.30 

1.33 

i  -.76 

1 

.13 

.62 

33.30 

1 

.15 

.98 

-.12 

30.03 

1 

|  -.63 

.00 

-.78 

0.65 

i  .00 

.03 

.03 

1.00 
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The  program  now  tries  to  make  a  plan  to  move  the  object  from  the  intermediate 
position  to  the  final  position: 

£1  -  30.00  30.00  .65  1.00 

Qi  -  .19  .98  .00  1.00 

Range  From  19  to  156  degrees 
Shifted  Range  From  -71  to  66  degrees 

E2  -  40.00  20.30  1.30  1.00 

Q2  -  .00  1.00  .00  1.00 

Range  From  7  to  120  degrees 
Common  range  From  7  to  66  degrees 
Approach  •  36  degrees 

Once  again  a  common  range  exists  and  the  program  calculates  the  two  hand 
positions: 


-.79 

.19 

-.58 

30.00 

.15 

.98 

.11 

30.00 

.59 

.00 

-.81 

.65 

.00 

.00 

.00 

1.00 

-.59 

.00 

.81 

40.00 

.00 

1.00 

.00 

20.00 

-.81 

.00 

-.59 

1.30 

.00 

.00 

.00 

1.00 

The  problem  is  solved  and  the  moves  can  be  made. 

By  this  procedure  it  is  possible  to  make  any  re-posi  tionings  and  re¬ 

orientations,  even  when  the  goal  must  be  accomplished  in  two  moves. 

About  4  seconds  are  required  to  compute  the  arm  positions  when  two  moves  must  be 
used. 
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SECTION  5 
TRAJECTORIES 


5.1  GENERAL  CONSIDERATIONS 

In  moving  the  arm  we  have  two  positions,  the  initial  and  final.  The  discussion 
until  now  has  emphasized  these  positions  and  their  determination  (Subsection 
4.2).  In  this  section  we  will  describe  the  move  in  detail. 

The  simplest  solution  is  to  move  the  joints  independently  from  their  initial 
position  to  their  final  position,  using  a  simple  servo.  Consider  the  situation 
shown  in  Figure  5.1,  where  the  hand  is  turning  a  block  onto  its  side.  The 
motion  is  mostly  in  joint  5;  if  all  the  joints  were  moved  to  their  final 
positions  then  the  hand  would  try  to  move  through  the  support.  Uhat  is  needed 
is  to  lift  the  arm  up  and  down  as  joint  5  i3  moved,  in  order  to  clear  the 
support.  When  the  arm  starts  to  move,  it  is  normally  working  with  respect  to 
some  surface,  for  instance,  picking  up  a  block  from  a  table  As  it  starts  to 
move  the  motion  of  the  hand  should  be  directly  away  from  the  surface.  If  we 
were  to  specify  a  position  on  a  normal  to  the  surface  out  from  the  initial 
position,  and  then  to  require  that  the  hand  pass  through  this  position,  we  would 
achieve  the  correct  departure  motion.  If  we  could  further  specify  the  time 
required  to  reach  this  position,  we  could  control  the  speed  at  which  the  block 
was  to  be  I i f ted. 


BEFORE 


AFTER 


Figure  5.1 
Crash 


To  estimate  how  far  this  position  should  be  from  the  surface,  consider  Figure 
5.2,  as  this  represents  the  worst  case  of  surface  penetration.  If  the  hand  had 
been  lifted  by  the  maximum  surface  penetration,  approximately  C5%  the  length  of 
the  last  link,  the  collision  would  have  been  avoided. 

For  such  an  initial  move,  the  differential  change  of  joint  angles  is  calculated 
ibubsection  2.3)  for  a  move  of  3  inches  in  the  direction  of  the  outward  pointing 
normal.  A  time  to  reach  this  position  based  on  a  low  arm  force  is  then 
calculated.  The  same  set  of  requirements  exists  in  the  case  of  the  final 
position.  Here  we  wish  once  again  to  approach  the  surface  in  the  direction  of 
the  normal,  this  time  passing  down  through  a  letdown  point. 

Ue  now  have  4  positions:  ini  t  ial ,  I  i  f  toff .  letdown,  and  final  and  if  we  were  to 
servo  the  arm  from  one  position  to  the  next  we  would  not  collide  with  the 
support  (see  Figure  5.3). 


TIME 


Figure  5.3 

Point  to  Point  Trajectory 


We  would,  however,  like  the  arm  to  start  and  end  its  motion  with  zero  velocity 
and  acceleration,  Further,  there  is  no  need  to  stop  the  arm  at  all  the 
intermediate  positions.  We  require  only  that  the  joints  of  the  arm  pass  through 

the  trajectory  points  corresponding  to  these  intermediate  positions  at  the  same 
1 1  me. 

The  time  for  the  arm  to  move  through  each  trajectory  segment  is  calculated  as 
follows:  for  the  initial  and  final  segments  the  time  is  based  on  the  rate  of 
approach  of  the  hand  to  the  surface  and  is  some  fixed  constant.  The  time 


GENERAL 


Figure  5.2 

flax i mum  Penetration  of  Hand 


FINGERTIP 

TRAJECTORY 


MAXIMUM 

PENETRATION 
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GENERAL 
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necessary  for  each  joint  to  move  through  its  mid  trajectory  segment  is 
estimated,  based  on  a  maximum  joint  valocity  and  acceleration.  The  maximum  of 
these  times  is  then  used  for  all  the  joints  to  move  through  the  mid  trajectory 
segment. 

Knowing  the  joint  variables  and  times  we  can  determine  a  polynomial  for  each 
joint,  expressing  joint  angle  as  a  function  of  time,  which  passes  through  all 
the  points  and  has  zero  initial  and  final  velocity  and  acceleration;  as  there 
are  4  points  and  4  velocity  and  acceleration  constraints  we  would  need  a  7th. 
order  polynomial.  Although  such  polynomials  satisfy  our  conditions,  they  often 
have  extrema  between  the  initial  and  final  points  and  the  joint  variable  must  be 
evaluated  at  each  extremum  to  check  that  it  has  not  exceeded  the  working  range 
of  the  joint. 

As  the  extrema  are  difficult  to  evaluate  for  such  high  order  polynomials,  ue  use 
a  different  approach.  Ue  specify  three  polynomials  for  each  joint,  one  for  the 
trajectory  from  the  initial  point  to  the  liftoff  point,  a  second  from  the 
liftoff  to  the  setdown  point,  and  a  third  from  the  setdown  to  the  final  point. 
Ue  specify  that  velocity  and  acceleration  should  be  zero  at  the  initial  and 
f  inal  points  and  that  they  should  be  continuous  at  the  intermediate  points. 
This  sequence  of  polynomials  satisfies  our  conditions  for  a  trajectory  and  has 
extrema  which  are  easily  evaluated.  „  -  * 

If  a  joint  exceeds  its  working  range  at  an  extremum,  then  the  trajectory  segment 
in  which  it  occurs  is  split  in  two,  a  new  intermediate  point  equal  to  the  joint 
range  limit  is  specified  at  the  break,  and  the  trajectory  recalculated  (See 
F igure  5.4) . 

Although  a  collision  avoider  has  not  been  implemented,  except  in  the  case  of  the 
table  and  the  arm  support  post,  such  a  program  would  modify  the  arm  trajectory 
in  the  same  manner  by  specifying  additional  intermediate  points.  If  a  potential 
cci'ision  were  detected  one  or  more  joints  would  be  required  to  pass  through 
some  additional  trajectory  points  in  order  to  avoid  the  collision. 

Ue  have  another  type  of  trajectory  that  we  wish  to  be  ably  to  compute,  one  which 
moves  the  arm  along  a  well  defined  space  curve.  Here  we  obtain  a  sequence  of 
joint  angles  at  points  along  the  space  curve.  The  velocity  along  the  space 
curve  is  controlled  by  relating  distance  along  the  curve  to  time  between  points 
(See  primitive  DRAU  Subsection  7.2).  This  type  of  curve  leads  to  a  trajectory 
with  many  points.  If  we  were  to  use  a  single  polynomial  it  would  need  to  be  of 
high  order,  for  this  reason  the  sequence  of  low  order  polynomials  is  also 
preferred. 


5.2  POLYNOMIALS 

For  each  trajectory  segment  we  have  position,  velocity  and  acceleration 
constraints  at  each  end.  Except  at  the  beginning  and  end  of  the  trajectory  the 
velocity  and  acceleration  constraints  are  continui ty  constraints.  There  are 


Figure  5.4 

Trajectory  beyond  Joint  Range 

only  four  constraints  on  the  intermediate  trajectory  segments  and  five 
constraints  at  the  ends.  Thus  for  the  first  and  last  trajectory  segments  a 
fourth  order  polynomial  uill  suffice  and  for  the  intermediate  trajectory 
segments  a  third  order  polynomial  uill  be  needed. 

Consider  a  trajectory  segment  described  by: 

4  3  2 

Q  »  Ai4*t'  +  A i3*t‘  +  Ai2*t'  +  Ail*t'  +Ai8 

(Eq.  5.1] 


u  i  th 


-  t/ri  [Eq.  5.2] 


where  ^  A  i  j  is  the  coefficient  of  the  j  th.  power  of  the  i  th.  trajectory  segment, 
and  time  t,  is  normalized  to  unity  at  the  end  of  the  trajectory  segment  of 
durat i on  r i . 

For  the  first  trajectory  segment  at  time  t'-8: 
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POLVNOMIALS 


00  -  Aie 
90-0  -All 
90-0  -A12 


and  at  time  t ' -1 s 

91  -  90  -  491  -  A14  +  A 13 

d*01  -  4*A14  +  3*A13 
2  .. 

rl  *91  -  12*A14  +  6*A13 


and  at  time  t" --Is 

9n  -  9(n-l)  -  A9n  -  -An4  +  An3 

rn*9(n-l)  -  -4*An4  +  3*An3 
2  .. 

tn  *9(n-l)  -  12*An4  -  6*An3 


[Eq.  5.3] 
CEq .  5.4] 
[Eq.  5.5] 

CEq.  5.6] 
[Eq.  5.7] 
[Eq.  5.8] 

[Eq.  5.9] 
[Eq.  5.10] 
[Eq.  5.11] 
[Eq.  5.12] 

[Eq.  5.13] 
[Eq.  5.14] 
[Eq.  5.155 


For  the  general  ith.  trajectory  segment  we  have 

0  -  Ai4 
9 ( i -1 )  -  A i 0 

t  i  *9  ( i »!,  5  -  Ail 
2  .. 

ti  *8i (-1)  «  A 12 

9i  -9(i-l)  -  A9i  -Ai3+Ai2+Ail 
• 

ri*9i  -  3*Ai3  +  2*Ai2  -t-Ail 
2  .. 

ri  *9i  -  6*Ai3  +  2*Ail 


[Eq.  5.16] 
[Eq.  5.17] 

[Eq.  5.18] 

CEq.  5.19] 
[Eq.  5.20] 

CEq.  5.21] 

[Eq.  5.22] 
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POLYNOMIALS 


Equations  5.3,  5.4,  5.5,  5.10,  5.11,  5.12,  and  Equations  5.17  specify 

coefficients  directly.  The  remaining  coefficients  may  be  solved  in  the  form: 


1 

491  1 

1 

1 

1  1  1 

1 

1 

1 

I  A13 

1 

1 

0.  1 

I 3/r 1  4/rl  | 

-l/r2 

1 

1 

| 

1 

|  A14 

1 

i  2  2| 

2 

1 

| 

| 

0  1 

IB/rl  12/cl  | 

j  i 

0 

-2/c2 

1 

I  A21 

492  | 

1 

1  1 

1  1 

i  i 
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1 

1 

1  | 

1 

1 

| 

I 

I  A22 

j 

0  1 

l  l 

m  1  1 

l/r2 

2/r2 

21x2  | 

1 

1  * 

I  A23 

1 

2 

2| 

1 

| 

| 

1 

1  1 

21x2 

21x2  \ 

1 

| 

| 

II  II  1 

1 

1 

II  II 

| 

1 

| 

1 

1 

I  li  II 

| 

1 

0  1 

1 

1 

1 

1 

1 

1 

1 

1 

-3/tn  4/cn  | 

1 

1 

1 

1 

1 

1 

1 

2  2 1 

1 

0  1 

1 

| 

1 

J 

S/rn  -12/cn  j 

| 

|  An3 

I 

49n  | 

1 

I 

1 

1 

1 

1  -1  I 

1 

1 

|  An4 

I 

tEq.  5.231 


►.here  the  blocks  indicated  in  Equation  5.23  may  be  repeated  for  each  additional 
point  that  the  trajectory  must  pass  through. 

Normalized  time  t'  runs  from  0  to  1  for  each  trajectory  segment  except  for  the 
last  segment  in  which  case  normalized  time  t* '  runs  from  -1  to  0.  The  arm  servo 
program  requires  that  normalized  time  t'  run  from  0  to  1  for  all  trajectory 
segments.  If  we  substitute: 

f  a  t"  +  i  tEq.  5.24] 


in: 

4  3  2 

9  -  A4n*t' 1  +  A3n#t"  +  A2n*t' '  +  Aln*t''  +  A0n 

tEq.  5.25] 

we  obtain: 


”  M*  MCtW«MKilK 
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4 

9  -  A4n*t' 

3 

+(-4#A4n  +  A3n)*t' 

2 

+(6*A4n  -3*A3n  +A2n)#t’ 

+(-4#A4n  +3*A3n  -2*A2n  +Aln)*t' 
+(A4n  -A3n  +A2n  -Ain  +A0n) 


[Eq.  5.283 


and  this  gives  us  the  coefficients  of  a  polynomial 
segment  in  uhich  normalized  time  runs  from  0  to  *  as 
program. 


for  the  last 
required  for 


trajectory 
the  servo 


5.3  TRAJECTORY  EXTREMA 

Intermediate  trajectory  segments  are  described  by  third  order  polynomials,  one 
such  polynomial  for  each  joint: 


3  2 

9  .  Ai3*t'  +  A i 2* t '  +  A i 1 * t '  +Ai0 


IEq.  5.27] 


the  derivative  is: 


9  -  3*Ai3*t'  +  2#A i 2# t '  +  Ail 


[Eq.  5.781 


The  times  of  the  extrema  are  given  as  the  roots  of  Equation  5.28: 

2  1/2 

t‘  -  -1/3  A i 2/A i 3  ±  1(1/3  A i 2/ A i 3)  -  1/3  A i 1 / A i 3 ) 


[Eq.  5.29] 


If  the  discriminant  is  positive  and  0  <  t'  <  1  then  an  extremum  exists  and  it 

can  be  evaluated  by  Equation  5.27. 


In  the  case  of  the  initial  and  final 
described  by  fourth  order  polynomials 
missing  (see  Equation  5.4,  5.5,  5.11, 


trajectory  segments  the  traj 
(Equation  5.1)  with  the  low 
and  5.12). 


ectories  are 
order  terms 


>*,na *****  •' 
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4  3 

9  ■  Ai4*t'  +  Ai3*t'  +  A i 0 


CEq.  5.30] 


and  the  derivative  by: 


3  2 

9  -  4#Ai4#t'  +  3*A i 3*t 1 

The  time  of  the  extremum  is  given  as  the  roots  of  Equation  5.31: 

t'  -  -3/4*A i 3/A i 4 


lEq.  5.313 


IEq.  5.323 


if  0  <  t'  <  1  than  an  extremum  exists  and  th*  value  of  9  is: 

n 

01 

9  -  -l/4wA).J*t ' 


tEq.  5.333 


In  the  case  of  the  first  and  last  segments  we  also  require  monotonic  motion  in 
order  to  avoid  overshoot.  To  ensure  monotonicity  we  compare  the  value  at* the 
extremum  to  the  initial  or  final  point  instead  of  to  the  physlcl | I  i ml t. 


5.4  CONTINUOUS  MOTION 

a?eSUob?aineri  I?  rl  tr?ject?ries  space  curves,  where  arm  solution. 

KnnS^fl!  th^:r^i;?ra,Lr?stsespeci;?  s.  '? the" 

armWHotionhoffFrSt  V  positions  are  the  same,  for  example  the Jcircu l'ar 

the  or  -F'9Ure  ,5.5.  The  position  of  the  hand  is  described  by  an  ang  e- 

the  orientation  remains  the  same  during  the  motion  Foinfinne  u*  •  e* 

every  60  degrees  so  that  the  hand  will  approximately  follow  the  path°  of ''the 

s'si"'  --  -•  «» addi«?Lfhepopiuion  ir : 

ape „ i  t  i eo,  d  and  h,  at  20  and  343  degrees  resDeetivelu  Tw0  „•  , . 

froin  ?«st°d  .°f  of  the  8  secJnents'  and  thus  the  arm  nil  accelerate 

sea,,  segment^ensur^shooth  departure^and^arrtval.  Th'  ,irs'  «• 

circular  &  SSiS  " 

it  to  move  along  the  alternate  trajectoru  "  potn  -g  °  Uhan  t  ?.  ??U*'?9 

stop  it  is  not  divarted  at  th.  "suitch"  it  position  "g"  but  al  ouid  if  ,'° 
moving  through  position  "h"  to  "a."  3  a  owed  to  stop  by 
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In  the  f o I  lowing 
5.23.  Equation  I 
is  in  the  case  o 
order  trajectory 


matrix  equation  the  x' s  represent  non  zero  elements  of  Equation 
.34  is  for  the  case  of  a  simple  trajectory  and  Equation  5.35 
looping,  providing  for  the  solution  of  tuo  additional  fourth 
segments  9  and  10. 
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LOOPING 

401 

XX 

0 

XXX 

0 

XX  X 

A62 

XXX 

0 

XXXX 

0 

XX  X 

403 

XXX 

0 

XXXX 

0 

XX  X 

494 

j  * 

XXX 

0 

XXXX 

0 

XX  X 

403 

XXX 

0 

XXXX 

0 

XX  X 

49G 

XXX 

0 

XXXX 

0 

XX  X 

407 

XXX 

0 

xxxxx 

0 

XXXX 

498 

XX 

I  A13  | 

I  A14  | 

I  A2I  | 

I  A22  | 

I  A2o  | 

I  A31  | 

I  A32  | 

I  A33  | 

I  A41  | 

I  A42  | 

I  A43  | 

I  A51  I 
I  A52  | 

I  A53  | 

I  AG1  | 

I  AG2  | 

I  AG3  | 

I  A7i  | 

I  *72  | 

I  A73  | 

I  AS3  | 

I  A84  | 

[Eq.  5.34] 


of  looping,  the  blocks  of  elements  marked  as  Y' s  and  Z' s  are  the 
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I  A01 
I  0 
I  0 
I  A02 
I  0 
I  0 
I  A03 
I  0 
I  0 

|  ASA 

I  0 
I  0 
I  ABB 
I  0 
I  0 
A0G 
0 
0 

A07 

0 

0 

A08 

0 

0 

A09 

0 

0 

AS10 

0 

0 


XX 
XXX 
XX  X 
XXX 
XXXY 
XX  Y 
XXX 

xxxx 

XX  X 
XXX 

xxxx 


A  1 

1  A51 

XXX  1 

*  1  A52 

xxxx 

1  A53 

XX  X 

1  A61 

XXX  1 

1  A62 

zzzx 

I  AB3 

ZZ  X 

I  A71 

XXX  | 

1  A72 

xxxxx 

1  A73 

XXXX  ; 

|  A83 

XX  | 

I  A84 

ZZZ  X 

I  A91 

ZZ  X  I 

1  A92 

XXXX  I 

I  A93 

XXXXX  I 

I  A94 

XXX  X  I 

1  A101 

XXXX  1 

1  A102 

XXXX  1 

I  A103 

XXX  1 

I  A104 

fEq.  5.351 


Equation  5.35  is  the  same  as  Equation  5.34  except  for  the  artrii  +  inn 

eight  equations  for  segments  9  and  10  These  are  *n,,r+k  „  ^  *  on  °f  *he  .act 

the  extra  end  continuitu  constraints  Th! \  f°U"th  °rder  Poiynomials  due  to 

lower  tr  i  angle  “ Equal  ion  !?ls  rl"lt  .  Mh  !  ■,™  2er°  >n  >h* 

g  and  c  (see  Figure  5.5).  *  continuity  requirements  at  points 


5.5  DIFFERENTIAL  NOTION 

The  arm  can  also  make  differential  motions  where  all  six  inint* 
change  a  certain  amount  in  a  given  time  In  this  rase  *  ?  required  *o 

together  to  make  the  change  in  tfle  speci  f  ied  ime  The  rJ  6  J°":tS  are  driven 
are  determined  as  in  Subsection  2.3.  P  t,Pe’  Th®  chanses  of  angles 

Joint  angle  0  as  a  function  g(f)  of  normal ired  time  t'.  for  a  change  of  40  is, 
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t'  ■  t/rm 
3  2 

9.g(t').AP*t‘  (  GV  -  15t'  +  10) 


This  gives  zero  initial  and  final  acceleration  and  velocity. 

This  type  of  motion  has  all  the  undesirable  properties  described 
5.1  and  is  only  suitable  when  small  changes  are  to  be  made,  as  in 
hand  position  during  visual  servoing.  Such  motions  are  of  course 
to  plan  than  regular  trajectory  controlled  motion. 


[Eq.  5.3G) 
tEq.  5.37] 

in  Subsection 
orrecting  the 
much  simpler 
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SECTION  6 
SERVO 


In  this  section  ue  relate  position  error  to  joint  torque  in  the  arm  servo.  Ue 
then  discuss  the  model  of  the  joint  drive  in  order  to  convert  joint  torque  into 
motor  drive.  In  the  final  section  provision  is  made  for  degrees  of  freedom  of 
the  arm  as  it  is  servoed. 


8.1  FEEDBACK  LOOP 

In  this  section  ue  uill  describe  the  servo  response.  Ue  will  treat  the  system 
as  continuous,  and  uill  ignore  the  effects  of  sampling,  assuming  that  the 
sampling  period  is  much  less  than  the  response  time  of  the  arm.  At  the  end  of 
the  section  ue  uill  check  that  this  assumption  is  valid.  Time  is  normalized  to 
the  sampling  period,  uhich  has  the  effect  of  scaling  the  link  inertia  up  by  ft2 
where  f  is  the  sampling  frequency.  The  Laplace  transform  is  used  throughout  and 
is  represented  by  F(s). 

The  set  point  for  each  joint  of  the  arm  is  obtained  by  evaluating  the 
appropriate  trajectory  segment  polynomial  for  the  required  time.  The  velocity 
and  acceleration  are  evaluated  as  the  first  and  second  derivatives  of  the 
polynomials. 

The  posi  tion  error  is  the  observed  position  B  less  the  required  value  Bs. 
Likewise  the  velocity  error  is  the  observed  velocity  less  the  required  velocity. 
Position  feedback  is  applied  to  decrease  position  error  and  velocity  feedback  is 
used  to  provide  damping. 

2 

The  feedback  loop  i6  shoun  in  Figure  6.1  The  arm  is  represented  by  ^/s  J,  uhere 
J  is  the  effective  link  inertia  a  function  of  arm  configuration.  T(s)  is  an 
external  disturbing  torque.  The  set  point  R(s)  is  subtracted  from  the  current 
position  to  obtain  the  position  error  E (s)  and  is  multiplied  by  representing 
differentiation,  to  obtain  the  error  velocity.  There  are  tuo  feedback  gains  ke 
and  kv,  position  and  velocity  respectively. 

By  uriting  the  loop  equation  ue  can  obtain  the  system  response: 


E(s)«  (-s  J) / (s  J  +  skv  +  ke)*R (s)  +  1/ (s  J  +  skv  +  ke)*T(s) 

tEq.  G.l] 


and  the  condition  for  critical  damping  is: 

1/2 

kv  «  2(J*ke)  [Eq.  6.21 
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T(«) 


Chans,,  H.  are  enable  to  .aintain  a  Jivan  r„p“,r  ,,«  ISCli?nn^“ 

f?  i?:n!rr«s:„^i  oS?;fA?uration- u  -  -  •  *•«"  •*  -J  » js^i 


2  2  2 

E(s)»  (-S  )/(£  +  skv  +  ke)*R(s)  +  1/ (s  +  skv  +  ke)*T(s)/J 


TEq.  B. 


and  the  condition  for  critical  damping  iss 


kv  =  2*(ke) 


1/2 


lEq.  6. 


It  can  be  seen  that  the  servo  response  is  now  independent  of  arm  configuration. 

The  principal  disturbing  torque  is  that  due  to  gravity,  causino  a  lame  „„ 
error,  especially  in  the  case  of  joint  2.  If  we  were  able  tn  aL  a  f  9  Position 

saV^en?  ?Aai  f^ou^d T*  *T 

ra  ,?;rbi"3  —  «•»  rrecz,J^  ;hh: 
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E(s)-  (-8  ) / ( s  +  skv  +  ke)*R(s)  +  1/ <s  +  ekv  +  ke)*Te(e)/J 

tEq.  6.51 


Ue  can  compensate  for  the  effect  of  acceleration  of  the  eat  point  R(s),  the 

firet  term  in  Equation  6.5.  if  ue  add  a  term  s  R(e)  (see  Figure  6.4)  and  obtain 
a  eystem  reeponse: 


2 

E(s)»  1/ (s  +  ekv  +  ke)*T(e)/J  IEq.  6.61 


The  gain  of  -J  and  the  torque  Tg  are  obtained  from  Equation  2.35  which  ue 
restate  here: 

Fi  -  E  f  Traced  Ujk|*j  Hj|*|  Uji  j  *qk) 
j-i  k-1 

+1  !  I  Trace 1 1  Ujkp|*|  Hj|*|  Uji|  *qj*qk) 

j-i  k-1  p-1 

-E  mj*|  G  |*|  Uji  |*|  Rj  [Eq  2.361 

j-i 

Uhat  ie  deeired  ie  to  obtain  an  expression  of  the  form: 

•  i 

Fi  ■  Ji  *  qi  +  Tg  (Eq.  6.7] 


given  an  arm  configuration  qi. 

The  velocity  dependent  terms  of  Equation  2.36  are  only  significant  at  high  6peed 
and  are  email  for  the  arm  ue  are  using.  Ueuill  ignore  the  eecond  term  of 
Equation  2.36,  although  it  could  be  included  uith  the  third  term  as  the  values 
•  • 

of  qj  and  qk  are  knoun  from  the  trajectory. 

Ue  may  interchange  the  order  of  t  .cion  of  Equation  2.36  to  obtain: 
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6  B  7 

Fi  “  .E.Trace(l  Ujkj*|Hj|*|Ujif  >*qk 

k-lj-i 


-r  mj*|  G  |*|  Uji|*|  Rj 
j-i 


CEq.  6.8] 


Fi  ■  I  Cik  qk  +  Ci 
k*l 


lEq.  6.9] 


where: 


6  T 

C •  k»  E  Trace! j  Ujk|*|  Hj|*|  Uji|  ) 
j-i 


CEq.  6.10] 


Ci  -  -E  mj*|  G  |*|  Uj i |*|  Rj 
J-i 


CEq.  6.11] 


2Erchui^h'a,t;xia:r«r°s  i,sanfttr=;si2!i,r,sard  ,he  ,er,s  •*  ***'•«  « " 


where: 


Fi  -  Ji  *  qi  +  Tg 


6  T 

Ji  --  E  Trace! |  Uji|*j  Hjj*|  Uji|  ) 


CEq.  6.123 


CEq.  6.13] 


and  Tg  as  Ci  in  Equation  6.11 

Ji  is  the  effective  joint  inertia  and  Tg  is  the  constant  term  due  to  gravity. 


Figure  6.4 

Acceleration  Compensated  Loop 


fc. .^oiWf'55* 
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Figure  6.5 

Position  Error  with  Inertia  Compensati 


i  ...  «.  -■  mw*  « — «#»  ■  **■. 
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Three  plots  of  position  error  v.  time  for  joint  2  are  shown:  Figure  6.5.  Fiaure 
6.6,  and  Figure  6.7.  ■ 

In  Figure  6.5  there  is  no  gravity  or  feedback  compensation,  in  Figure  6.6  the 
gravity  term  is  added,  and  in  Figure  6.7  the  acceleration  compensation  is 
included. 

The  steady  state  error  for  a  given  error  torque  Te  is: 

Ess  -  Te/ (  J  *  ke  )  CEq.  6.14] 


We  would  like  the  arm  to  be  stiff  when  it  is  servoing  and  a  value  of  50  oz/in  at 
the  hand  is  the  minimum  acceptable,  if  a  lesser  value  is  used  the  arm  moves  very 
sloppily.  For  this  value  of  stiffness  the  values  of  ke  can  be  calculated  by 
means  of  Equation  6.14.  Representative  values  of  kv  based  on  critical  damping 
may  then  be  calculated  (Equation  6.4)  and  will  give  us  the  lower  limits  on  ke. 
and  kv. 

There  is,  however,  some  noise  in  position  measurement  and  if  ue  are  to  obtain 
velocity  by  differencing  observed  position  readings  then  the  same  noi'je  is  in 
the  velocity  determination.  The  primary  source  of  noise  is  the  quantizing  noise 
of  the  analog  to  digital  converter  used  to  determine  position.  The  A/D  converter 
is  a  12  bit  device  and  thus  has  a  relative  error  of  1/4096.  The  equivalent 
noise  torque  at  the  joint  is  given  by: 

Tn  -  k  *  J  *  e  [Eq.  B.  151 


where  k  is  either  ke  if  the  velocity  is  determined  separately,  or  ke+kv  if  the 
velocity  is  determined  by  differencing,  and  e  is  the  quantizing  noise.  A  noise 
torque  of  approximately  F0/2  (see  Subsection  6.2)  seems  to  be  acceptable  and 
this  gives  an  upper  bound  on  k. 

Based  on  this  information  we  can  evaluate  Equations  6.14,  6.4  and  6.15  for  each 
joint: 

Table  6.1 

Ranges  of  Servo  Gains 


JOINT 

te 

& 

& 

1 

0.038 

0.39 

0.18 

2 

0.032 

0.36 

0.33 

3 

0.005 

0.14 

0.75 

4 

0.06 

0.49 

0.49 

5 

0.06 

0.49 

1.6 

6 

0.06 

0.49 

50.0 

The  second  column  is  based  on  Equation  3.14  and  the  third  on  Equation  6.4  These 
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Figure  6.7 

Position  Error  with  Acceleration  Compensation 
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are  the  minimum  acceptable  gains.  The  fourth  column  is  from  Equation  8. IS  and 
represents  the  maximum  allowable  gain  if  the  information  is  to  be  obtained  by 
means  of  position  measurement  using  the  available  A/D  converter.  In  the  case  of 
Joints  1  and  2  it  can  be  seen  that  an  alternative  means  must  be  used  to  obtain 
velocity  information  although  the  position  information  i6  acceptable  using  the 
A/D  converter.  To  measure  the  velocity  for  these  two  joints  a  tachometer 
generator  is  used,  which  has  much  lower  noise  than  velocity  obtained  by 
differencing  position  measurements. 

Ue  made  an  assumption  at  the  beginning  this  section  that  the  sampling 
frequency  was  much  higher  than  the  frequency  response  of  the  arm.  Ue  can  obtain 
the  frequency  response  from  Equation  E.6  by  finding  the  inverse  transform.  This 
is  'ound  to  be: 


frequency  =  kv  *  f  /  2 


CEq.  6.16) 


As  maximum  kv  that  we  require  is  0.49  this  give  us  a  response  of  1/4  the 
sampling  frequency.  It  is  found  in  practice  that  the  velocity  gain  can  be 
increased  to  1  before  the  effects  of  sampling  become  apparent. 

At  the  beginning  and  end  of  each  trajectory  and  at  tns  end  of  each  trajectory 
segment,  the  values  of  Ji  and  Tg  are  evaluated  (Equation  6.12).  They  are  then 
given  to  the  servo  together  with  the  part  trajectory  polynomial  coefficients.  As 
the  trajectory  is  executed  the  values  of  Ji  and  Tg  are  linearly  interpolated. 

For  the  hand  in  the  position  considered  in  the  example  in  Subsection  2.1  the 
values  of  Ji  and  Tg  are  (in  the  oz.  in.  l/60th.  second  system  of  units): 

Table  6.2 

Servo  Parameters 


JOINT 

Ji 

Tg 

1 

761000 

0 

2 

953003 

1419 

3 

9500 

-53 

4 

83030 

94 

5 

82000 

37 

6 

4003 

0 

Although  these  gains  give  an  acceptable  response  from  the  point  of  view  of 
stiffness,  the  gain  is  too  low  to  maintain  the  high  positional  tolerance  of 
±0.05  in,  which  we  are  just  able  to  measure  using  the  12  bit  A/D  converter.  In 
order  to  achieve  this  error  tolerance  the  position  error  is  integrated  when  the 
arm  has  reached  the  end  of  its  trajectory.  Uhen  the  position  error  of  j  joint 
is  within  tolerance  the  brake  for  that  joint  is  applied  and  the  joint  is  no 
longer  servoed.  Uhen  all  the  joints  are  within  the  error  tolerance  the 
trajectory  has  been  executed. 
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H  the  arm  is  to  move  a  heavy  load  its  predicted  effects  are  taken  into  account 
by  increasing  the  effective  mass  and  inertia  of  the  last  link  of  the  arm  before 
evaluating  Equation  6.12.  Si.ilarig.  i,  nano  i.  to  evert  a  g^en  Jorce  cr 

is  added  to  Tg. 


=  ^ . .  iyt  it  me  nana  is  to  e 

moment  then  the  equivalent  arm  torque  (see  Subsection  2.5) 


6.2  MOTOR  DRIVE 

The  output  of  the  servo  equation  is  a  torque  to  be  applied  at  the  ioint  Farh 
joint  has  an  electric  motor  drive  and  a  harmonic  drive  gear  redSc?  in  ?£e 
motors  are  driven  by  a  pulse-width  modulated  voltage  signal  The  outout  oi  IhI 

SM*!."-  ,he  po,ari,“- “!K. 

program°output  ' :pi„,Tr 


This  curve  can  be  explained  in  terms  of  two  friction  effects*  inari  * 
causing  the  ,uo  curves  to  diverge,  and  load  ind^InSenT  cluling  “parK  I; 
?!  *he  Th'  electrical  color  ,1,;  conltaSt  luo  ai^c? 

suool?ed  to  !fjh?  near  the  origm.  Evper i mental ly  determined  curvee  are 

supplied  to  the  servo  program  ,n  the  fol  losing  pieceuise  linear  form  (see  Figure 
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•TORQUE  >^>) 

VTORQUE  < 


OUTPUT  TORQUE 


PULSE  WIOTH 


Figure 

Piecewise  Linear  Pu 


6.9 

se  Width  v.  Torque 


where: 

V0  is  the  motor  drive  at  which  the  Joint  will  move  at  constant  velocity 
exerting  zero  force  in  the  direction  of  motion; 

F0  is  the  force  that  the  joint  will  exert  at  drive  level  V0  but  with  a 
negat i ve  veloci ty; 

the  slopes  and  slope  diffe'ences  are  obtained  from  the  experimental 
curves. 


Uhen  the  velocity  is  very  low  the  direction  of  intended  motion  is  substituted 
for  the  velocity. 


One  other  factor  considered  is  the  back  emf  of  the  motor.  The  value  of  h  is 
the  ratio  of  required  voltage  to  supply  voltage,  .he  supply  voltage  is  simply 
augmented  by  the  computed  back  emf  before  "h"  is  calculated. 


Uhen  the  velocity  is  non  zero  the  output  torque  is  predictable  but 
velocity  and  with  zero  intended  motion  the  error  in  the  output  orque  can 
much  as  half  the  horizontal  displacement  (F0/2)  between  the  two  curves 
origin.  The  values  for  this  error  torque  at  a  typical  arm  configuration  i 
of  force  at  the  hand  FH.  are: 


at  zero 
be  as 
at  the 
n  terms 


IMPtiNf 
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Table  G.3 


Arm 

Static 

Friction 

JOINT 

F0  02 

FH  oz. 

1 

400 

±10 

2 

700 

±20 

O 

70 

±35 

4 

100 

±5 

5 

160 

±8 

6 

100 

±70 

It  can  be  seen  that  the  arm  can  exert  forces  with  a  typical  tolerance  of  ±10  02 


G.3  PARTIALLY  CONSTRAINED  NOTION 

If  we  require  the  arm  to  exhibit  a  degree  of  freedom  along  a  given  direction  or 
about  a  given  axis  at  the  hand,  the  program  selects  a  joint  to  be  "free"  on  the 
following  basis.  The  program  calculates  the  equivalent  arm  torque  (Subsection 
for  a  unit  force  in  the  given  direction  or  moment  about  the  given  axis,  and 
then  normalizes  the  torques  by  dividing  through  by  their  respective  F0 
(Subsection  6..J.  The  program  then  selects  that  joint  which  has  the  laraest 
normalized  torque  to  be  "free."  This  is  the  joint  which  is  most  sensitive  to 
motion  in  the  required  direction  and  it  would  be  the  first  joint  to  move  if  the 
force  were  slowly  increased  from  zero  in  the  free  direction. 


If  we  require  more  degrees  of  freedom  we  repeat  the  process,  beina 
to  select  the  same  joint  twice. 


careful  not 


To  free  the  joint  during  motion,  the  feedback  gains  ke  and  kv  are  set  to  zero 
(see  Subsection  6.1).  This  means  that  the  free  joint  still  has  acceleration 
compensation  (Equation  6.6)  and  gravity  compensation  (Equation  S.5).  If  the  hand 
IS  required  to  exert  an  external  force  this  is  added  so  that  the  joint  is 
compensated  for  all  knoun  forces  and  has  no  feedback. 

The  free  joint  servo  response  ma* ■  be  obtained  from  Equation  6.6  as: 


E  (s)  -  Te/  (  J  *  s  ) 


lEq.  6.17) 


SECTION  7 
CONTROL 


In  addition  to  moving,  the  arm  can  perform  such  functions  as  opening  and  closing 
its  hand.  These  functions  and  motions  are  called  "primitives"  and  can  te  put 
together  to  make  an  "arm  program."  An  arm  program  is  assembled  to  make  a 
trajectory  file  specifying  the  primitives  together  with  supporting  data, 
tra jector ies,  effective  inertia  constants  and  gravity  loading  terms. 

Two  programs  exist,  one  for  assembling  "arm  programs"  and  the  other  for 
executing  the  resulting  trajectory  files. 


7.1  ARM  STATE 

The  state  of  the  arm  is  described  by  the  following  global  variables,  which  are 
located  in  the  upper  segment  and  are  available  to  all  other  programs  sharing 
this  segment,  such  as  a  strategy  program.  At  the  termination  of  execution  of  a 
program  the  state  variables  which  describe  the  arm  are  updated. 

ARM_MOTIGN  The  name  of  the  program  currently  being  executed.  This  is  a 
warning  flag  to  other  programs  that  the  arm  is  ’n  motion. 

ARMJJAIT  The  name  of  the  program  execution  of  which  is  temporarily 
suspended. 

ARM_STATUS  The  error  state  of  the  arm  at  the  end  of  execution. 

ARMJJNK  The  4x4  transform  which  describes  the  position  and  orientation 
of  the  hand  at  the  end  of  execution. 

GRASP  The  separation  between  the  finger  tips. 

ARM_SEGHENT  An  integer  variable  incremented  at  the  beginning  and  end  of 
each  MOVE  primitive. 

Programs  can  be  executed  by  the  following  two  procedures. 

00_IT  (NAME)  causes  the  program  NAME  to  be  executed. 

DQ_PROCEED  causes  continued  execution  of  the  program  execution  of  which 
was  interrupted. 

During  execution  various  errors  can  occur.  Although  some  errors  may  be  desired 
states,  they  are  known  as  errors  because  they  cause  the  arm  to  stop  and  to  apply 
all  the  brakes.  There  are  various  touch  sensors  on  the  arm  and  if,  when 
selected,  one  of  them  touches  anything,  an  "error"  occurs. 
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The  following  is  a  list  of  the  error  messages: 
xl  Excessive  force  at  joint  x. 

2  Hand  closed  beyond  minimum  specified  opening 
x6  Touch  sensor  x  was  touched. 

22  Excessive  force  at  hand 

23  Arm  failed  to  stop  on  specified  force. 

There  are  other  messages  which  refer  to  system  errors,  such  as  file  not  found. 

If  an  error  occurs,  execution  of  the  program  is  suspended  and  the  stat. 
variables  are  updated,  the  contents  of  ARflJIOTION  and  ARf1_.UA IT  are  exchanged  ano 
the  error  code  is  set  in  ARI1_STATUS.  Execution  can  proceed  uith  the  next 
primitive,  after  error  recovery,  by  calling  procedure  DO_PR0CEED. 


(F 


7.2  PRIfllTIVES 


This  section  lists  the  arm  primitives,  ..hich  have  meaning  at  two  times:  once  at 
assembly  when  the  trajectory  file  is  being  created  and  feasibility  must  be 
checked,  trajectories  planned  etc.,  and  once  at  execution  time  when  the 
primitives  are  executed  in  the  same  way  that  instructions  are  executed  in  a 
computer. 


OPEN  (DIST)  Plan  to  open  or  close  the  hand  such  that  the  gap  between  the 
finger  tips  is  01  ST. 


CLOSE  miNinum  Plan  to  close  the  hand  until  it  stops  closing  and  then 

?he  ga|3  betueen  the  f in0*r  tips  is  greater  than 
niNIMUn.  If  it  is  less,  then  give  error  2. 


CHANGE  (DX_DY_DZ.  VELOCITY)  Plan  to  move  the  arm  differentially 
(Subsection  5.5)  to  achieve  a  change  of  hand  position  of  vector 
DX_DY_QZ  at  a  maximum  speed  of  VELOCITY. 


SWEEP  (DIST,  VELOCITY)  Plan  to  move  the  hand  differentially  (Subsection 
5.5)  in  the  direction  of  the  hand's  orientation  vector  a 
distance  DIST  at  a  maximum  velocity  VELOCITY. 


LIFT  (DIST.  VELOCITY)  This  is  the  same  as  SUEEP  except  that  it  is  in  the 
direct  i  c  .  of  Q  x  m  where  0  and  A  are  the  orientation  and 
approach  vectors  respectively. 


REACH  (DIST,  VELOCITY)  Again  this  is  the  same  as  SUEEP  except  it  is  in 
the  direction  of  the  Approach  vector. 


-*#■**&* mmvmm 
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TILT  (ANGLE)  Plan  to  rotate  the  hand  di f ferent i a  I . j  ANGLE  degrees 
(Subsection  5.5)  about  the  orientation  vector. 

TURN  (ANGLE)  This  is  a  similar  rotation  about  Q  x  &. 

TUI  ST  (ANGLE)  Here  the  rotation  is  about  the  approach  vector. 

PLACE  Plan  to  move  the  hand  vertically  down  until  the  hand  meets  some 
resistance,  that  is,  the  minimum  resistance  that  the  arm  can 
reliably  detect. 

MOVE  (  T  )  At  assembly  time  check  that  the  position  specified  by  the 
hand  transformation  T  is  clear.  Plan  to  move  the  hand  along  a 
trajectory  from  its  present  position  to  |  T  | .  The  hand  is  moved 
up  through  a  point  LIFTOFF  given  by  LIFTOFF  ■  INITIAL  POSITION  + 
DEPART,  where  DEPART  is  a  global  vector  initialized  to  z  •  3 
inches.  Similarly  on  arrival  the  hand  is  moved  down  through  a 
point  SL-JMgive-  *y:  SEI..DQUM  •  FINAL  POSITION  »  ARRIVE. 
ARRIVE  is  also  set  *■  «.  •  3  inches. 

PARK  Plan  a  move  as  in  HOVE  but  to  the  "park"  position. 

DRAU  (DX  DY  DZ.  ROT  AXIS.  01,  CRANK.  CRANK  AXIS.  02,  TIME,  LOOPS,  FORCE, 
NUI1BER_FREE,  FREE  VECTOR)  This  is  a  trajectory  motion  of  the 
■land.  The  hand  is  at  the  end  of  a  vector  (CRANK)  which  is 
rotated  02  degrees  around  an  axis  (CRANK  AXIS)  as  its  origin  is 
translated  (OX  flv  OZ).  At  the  same  time  the  hand  is  re-oriented 
about  another  axis  (ROT  AXIS)  01  degrees.  See  Figure  7.1.  If  the 
end  point  is  the  same  as  the  initial  then  looping  may  be 
specified  (see  Subsection  5.2).  Finally  a  number  of  degrees  of 
freedom  and  an  excess  force  to  be  applied  during  the  program  may 
be  specified.  With  this  primitive  we  can  do  almost  anything! 

There  are  also  control  primitives  which  specify  how  the  other  primitives  are  lu 
be  carried  out. 

STOP  (FORCE,  MOMENT )  During  the  next  arm  motion  stop  the  arm  uhen  the 
feedback  force  is  greater  than  the  equivalent  joint  force 
(Subsection  2.5).  If  the  arm  fails  to  stop  for  this  reason 
before  the  end  of  the  motion,  generate  error  23. 

SKIPE  (ERROR)  If  error  ERROR  occurred  during  the  previous  primitive  then 
skip  the  next  primitive. 

SKIPN  (ERROR)  if  error  ERROR  occurred  during  the  previous  primitive 
execute  the  next  primitive  otherwise  skip  the  next  primitive. 

JUMP  (LAB)  Jump  to  the  primitive  whose  label  in  LAB. 

UAIT  Stop  execution,  update  the  state  variables  and  wait  for  a  proceed 
command. 
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Figure  7.1 
A  Draw  notion 


TOUCH  (MASK)  Enable  the  touch  sensors  specified  by  rask  for  the  next 
pr i mi t i ve. 

SAVE  Save  the  differential  deviation  from  the  trajectory  set  point. 
This  can  be  caused  by  CHANGE  type  primitives. 

RESTORE  Cajse  the  arm  to  deviate  from  the  trajectory  set  point  at  the 
end  of  the  next  motion  by  the  deviation  last  saved. 


7.3  ASSEMBLY  PROGRAM 

The  assembly  program  has  two  modes  of  input,  message  procedure  or  source  file. 
We  will  describe  the  source  file  input  here  although  it  should  be  kept  in  mind 
that  a  strategy  program  uould  be  able  to  execute  the  same  functions. 

Planning  normally  begins  from  the  current  position  and  proceeds  from  there.  The 
planned  state  cf  the  arm  is  kept  in  a  4x4  transformation  STATE.  Thus  when  a 
series  of  moves  is  planned  it  is  always  from  STATE  to  the  specif's-i  transform  T 
that  moves  are  made.  At  the  end  of  each  successful  move  STATE  is  updated.  In 
order  to  start  a  program  a  BEGIN  pseudo-op  must  be  assembled.  This  causes  a 
trajectory  file  to  be  named  and  specifies  the  initial  state  of  STATE.  Similarly 
at  the  end  of  a  program  the  trajectory  file  must  be  closed.  We  will  list  the 
pseudo-ops: 
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BEGIN  (FILE,  T)  Open  file  FILE  as  the  trajectory  file  and  initialize 
STATE  to  transformation  |  T  |. 

MERGE  Merge  the  last  assembled  primitive  with  the  preceding  motion 
pr  i ini  t i  ve. 

END  Close  the  trajectory  file. 

MACRO  (FILE)  Causes  input  to  the  planning  program  to  be  switched  to  FILE 
until  the  end  of  FILE.  This  gives  us  macros  without  parameters. 
The  MACROS  may  be  nested. 

In  the  case  of  most  primitivta  their  parameters  are  either  vectors  or 
transformations:  should  another  program  use  the  assembly  program  these  vectors 
and  transformations  would  be  data  structures  of  that  program.  In  the  case  of 
source  input  we  need  to  define  6uch  data  types  and  associate  them  with  symbolic 
names.  All  names  must  be  defined  before  a  primitive  can  be  assembled. 

Data  types: 

TRANS  (NAME.  R.  X,  Y,  Z,  Ox,  Qy,  Oz)  Set  up  a  4x4  transformation  NAME 
suc;i  that  it  has  position  x.y.z  and  orientation  vector  0x.0y.0z 
and  that  the  approach  vector  is  rotated  R  degrees  from  the 
reference  approach  vector  about  the  orientation  vector. 

VECT  (NAME,  x  ,  x  ,  x  ,  x  )  Define  a  vector  NAME  of  value  x  ,x  ,x  ,x  . 

1  2  3  4  1  2  3  4 

MOVE_INSTANCE  (TI,  TF,  IP)  This  is  partly  a  data  type  primitive  as  it 
sets  up  a  series  of  transformations  to  move  an  object  with 
transformation  TI  such  that  it  has  transformation  TF  using 
intermediate  position  iP  if  necessary.  However  it  assembles  all 
the  move  and  hand  primitives  to  accomplish  the  move. 

PROTOTYPE  (OBJECT)  this  sets  up  the  prototype  of  the  body  to  be  moved  bu 
MOVEJNSTANCE. 

The  planning  program  is  30K  and  shares  a  14K  segment  which  contains  runtime 
routines  and  global  data.  Typical  running  times  are  1  sec  to  plan  a  move,  and 
from  0.5  to  4  seconds  for  MQVE_IN5TANCE  depending  on  the  complexity  of  the  move. 


7.4  PROGRAMMING  EXAMPLES 

He  will  give  some  examples  of  hand  programs  to  clarify  the  use  of  primitives. 
The  first  example  is  to  move  the  hand  tc  a  position  20,30,1  to  pickup  an  object, 
then  to  move  it  to  40,20,2  and  place  it  on  ihe  table. 

BEGIN  TRANSFER  0 
TRANS  T 


.•oJEJjjg 
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90  20  30  1  1  0  0 

MOVE  T 
CLOSE  0.5 

TRANS  T 

90  40  20  2  1  0  0 

MOVE  T 
PLACE 
PARK 
OPEN  3 
MERGE 

END 


Set  up  a  transform  to 
pos i t ion  the  hand. 

If  there  is  nothing  here 
then  an  error  will  occur. 

Change  the  transform  to 
the  new  posi t ion. 


The  hand  will  open  as  it 
starts  to  move. 


The  next  example  is  to  grasp  an  object  without  moving  it 
error  in  its  position.  The  hand  is  closed  with  the  touch 
finger  touches  the  object.  The  hand  is  then  "swept"  and  c 
until  the  other  'inger  touches.  The  hand  is  then  closed. 


in  case  there  if,  some 
sensors  enabled  unt i I  a 
losed  in  0.1  inch  steps 


TOUCH  1 
OPEN  -1 

L2:  SKIPE  G 

JUMP  LI 
SWEEP  -0.1  1 
TOUCH  1 
OPEN  -1 
SKIPE  1G 
JUMP  L2 
JUMP  L3 


This  will  cause  the  hand  to 
close  wi th  touch  "on". 

Did  the  left  finger  touch? 

Yes  move  right. 

And  close  the  hand  again. 
Did  the  other  finger  touch? 
No  move  right  again. 

Yes  a  I  I  done. 


LI:  SKIPE  1G 

JUMP  L8 
SWEEP  0.1  1 
TOUCH  1 
OPEN  -1 
SKIPE  6 
JUMP  LI 

L3:  CLOSE  0.5 

JUMP  L10 
L8:  WAIT 

L10: 


Check  that  the  right  finger 
touched. 

No  then  some  error. 

Move  left. 

And  close  the  hand  again. 
Did  the  left  finger  touch? 
No  move  again 
Yes  close  the  hand. 

And  finish. 

The  error  state. 


The  I  ast _ examp  I e,  whose  file  name  is  CATCH,  has  no  begin  and  end  as  we  are  goina 
to  use  it  as  a  macro.  Ue  will  use  CATCH  first  to  position  the  hand  on  the 
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object,  then  to  turn  it  98  degrees 
will  be  picked  up  centrally. 


and  close  the  hand  again  so  that 


the  object 


MACRO  CATCH 


OPEN  3 
TUI  ST  90 

MACRO  CATCH 


Here  we  assume  that  the 
hand  has  been  positioned 
over  the  object.  The 
macro  call  will  substitute 
the  text  from  our  previous 
example. 

Now  open  the  hand  and  turn 
it  around  90  degrees. 

And  close  i t  again. 


tS'repla«"thiSCLDSF  t  *?cr;.cal1"1  p!«.  If  the  primitive  HACR0 


PICK  were 
would  have 
its  initial 


We  will  give  one  final  example,  that 
hanu  to  be  holding,  on  a  vertical  bolt 
when  the  torque  is  200  oz. in. 


is  to  put  a  nut,  which  we  wi I 
located  at  20,30,1.  Ue  will 


I  assume  the 
stop  turning 


BEGIN  SCREU  0 
TRANS  T 


90  20  30  2  1  0  0 

MOVE  T 

PLACE 


VECT  ROTATION 
0  0-1 
VECT  NULL 
0  0  0 

VECT  MOMENT 
0  0  -200 
STOP  NULL  MOMENT 


VECT  FORCE 
0  0  -20 


VECT  FREEX 
1  0  0 


VECT  FREEV 
0  1  0 
VECT  FREEZ 
0  0-1 


Move  the  nut  to  the  bolt 
and  place  i t. 

The  bolt  axis 


The  stopping  torque. 

Stop  the  arm  during  the 
motion  when  the  torque 
is  200  oz.  in. 

Ue  will  want  to  push 
down  on  tne  bolt  as  we 
turn. 

The  hand  must  be  free  in 
the  x,y  and  z  directions 
as  we  screw. 


DRAW 

NULL  ROTATION  358 
NULL  NULL  0 
50  3  FORCE 


3 

FREEX 

FREEY 

FREEZ 

END 


No  change  of  position 
No  crank  axis 
1  Second  per  turn,  plan 
for  a  maxi  mum  of  three 
turns.  Exert  the  force. 
Three  drgrees  of  freedom. 


This  will  screw  the  nut  on  the  bolt. 


7.5  EXECUTE 

With  the  exception  of  HOVE  and  DRAW,  which  require  trajectory  files,  most 
functions  can  be  executed  directly  by  prefixing  the  primitive  name  by  "DO."  The 
assembly  program  plans  the  action  and  sends  it  to  the  arm  servo  program  to  be 
executed.  This  does  not  change  the  state  of  the  arm  servo  program  if  it  is  in  a 
"wait"  state  and  execution  can  continue  after  any  number  of  executed  primitives. 
This  method  is  used  by  the  interactive  programs,  which  ui  1 1  plan  a  move  to  bring 
the  hand  close  to  the  required  place  and  then  plan  a  "uait."  When  executed,  the 
hand  position  will  be  modified  during  the  wait  phase  by  the  interacting  program 
executing  a  series  of  "DO"  commands.  Execution  of  the  preplanned  trajectory  can 
then  continue  by  calling  "D0_PR0CEED. " 


7.5  ARM  PROGRAM 

A  simplified  flou  chart  for  the  execution  program  is  shown  in  Figure  7.2.  The 
loop  is  executed  50  times  a  second.  If  the  arm  is  not  in  motion  then  RUN  is 
false  and  the  touch  sensors  are  checked  before  performing  any  function.  At  the 
completion  of  a  motion  or,  if  the  arm  is  not  moving  at  the  completion  of  a 
function,  the  program  counter  is  incremented  and  the  next  primitive  executed.  A 
zero  primitive  terminates  execution, 

The  block  ANGLES  measures  all  the  joint  angles  and  performs  a  piecewise  non¬ 
linear  conversion  on  them.  The  velocities  are  also  determined,  either  by  reading 
tne  tachometer  generator  outputs  (joints  1,2)  or  by  differencing  the  position 
information. 

The  block  SERVO  corresponds  to  Subsection  B.l{  here  the  errors  are  computed  and 
the  drive  torques  calculated,  If  a  stop  arm  primitive  is  in  effect  then  the 
error  torques  are  checked  against  the  equivalent  arm  torques  to  determine  if  the 
arm  should  be  stopped.  At  the  end  of  the  trajectory  the  position  errors  are 


nulled.  Uhen  each  individual  joint  is  within  the  error  tolerance  the  brake  is 
applied  and  when  all  joints  are  stopped  tne  next  primitive  is  executed. 


Section  DRIVE  takes  the  joint  torques  as  input  and  computes  the  motor  drive.  It 
checks  for  excessive  force  and  stops  the  arm. 


The  THETA  section  computes  the  next  values  of  the  joint  angles,  interpolates  th? 
Ji  and  Tg  values  (see  Equation  6. 12)  and  controls  the  switching  as  required  for 
looping. 

The  Touch  sensors  are  then  checked  and  if  any  have  been  touched  the  arm  is 
stopped.  Finally  the  function,  if  any,  is  performed.  Functions  are  not 
normally  performed  while  running  with  the  exception  of  hand  opening. 

The  execution  time  for  the  loop  is  approximately  8msec  on  the  PDP-6  processor, 
using  floating  point  hardware.  The  program  length  is  3K  words  including 
tra jectory  buffers. 

While  the  arm  is  running  the  trajectory  set  point  8t  is  given  by: 

9t  -  f(t')  CEq.  7.11 


where  f(t)  is  the  appropriate  trajectory  segment  polynomial  (Subsection  5.2)  and 
t*  ie  normalized  time.  The  arm  set  point  is  as  follows: 

0s  ■  0t  4  d0  [Eq.  7.2] 


where  d9  is  a  constant  offset  between  the  set  point  and  the  trajectory  point. 
Between  liftoff  and  set-down  for  a  period  of  time  Tm: 

0s  -  9t  +  d9  +d29*g(t' )  (Eq.  7.3] 


where  g ( t '  )  is  given  in  Equation  5.37.  Uhen  t  -  rm,  d9  is  changed  as  follows: 


d9  d9  4  d29 

(Eq. 

7.4] 

d29  «-  0 

(Eq. 

7.5] 

At  the  beginning  of  each  trajectory  motion  we  set: 

d0  «-  0  -  0t 

(Eq. 

7.G] 

d20  *■  -d9  +  d20 

(Eq. 

7.7] 

where  9  ie  the  observed  value. 

Thus  at  the  beginning  of  a  trajectory: 

9s  ■  9  ■  9t  +  d9 

(Eq. 

7.8] 

and  at  the  end  of  let-down: 

9s  -  9t  4  d29i 

(Eq. 

7.9] 
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where  d29i  is  the  value  of  d29  at  the  beginning  of  the  trajectory  Equation  7.7. 
If  d29  was  set  to  zero  at  the  end  of  the  previous  trajectory  by  Equation  7.5 
then  Equation  7.9  becomes: 


9s  -  9t  (Eq.  7.183 

This  means  that  if  the  arm  is  displaced  from  the  point  from  which  the  trajectory 
was  planned,  the  arm  is  gradually  brought  back  to  the  trajectory  during  the  mid- 
part  of  the  motion. 

All  the  differential  motions  are  accomplished  by  loading  d29  setting  Tm  and 
setting  RUN.  At  the  end  of  Tm  the  arm  has  moved  d29  and  the  value  of  d9  has  also 
been  incremented  by  d29. 

The  "save"  command  causes  d9  to  be  saved  and  the  “restore"  primitive  loads  d29 
with  the  previously  stored  d9.  By  this  means  we  can  find  some  location  by 
differential  motion,  save  the  j8,  and  then,  if  we  ui#h  to  return  to  this 
differentially  modified  position,  restore  d9  into  d2Q  before  returning  to  the 
unmodified  position. 
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SECTION  8 
CONCLUSIONS 


8.1  sunnARY 

In  this  work  we  have  attempted  to  provide  a  systematic  approach  to  arm 
programming.  Ue  have  been  concerneo  with  three  main  problems.  1)  how  to  position 
the  hand  on  an  object.  2)  how  to  move  the  hand  and  o)  hou  to  servo  the  arm. 

The  first  problem  has  been  solved  by  setting  up  a  world  model  to  represent  these 
objects:  problems  relat'ng  to  grasping  objects  in  the  world  model  are  solved 
symbol  ical  ly.  When  objects  are  locates,  either  by  the  vision  system  or  by  touch, 
they  are  represented  in  the  modei.  In  performing  the  difficult  manipulations 
r-quired  by  the  istant  Insanity  Purple  [Feldman  71b]  this  model  based  approach 
worked  without  failure.  The  arm  nas  functioned  in  a  c I  ear  env i ronmen  and  a 
collision  avoider  has  not  been  implemented.  It  is  hoped  that  the  world  model 
will  be  adequate  to  solve  the  collision  prcblem  symbolically. 

In  moving  the  arm  we  have  demonstrated  the  necessity  of  servoing  the  hand 
through  a  succession  of  positions  in  order  to  avoid  colliding  with  the  support. 
Having  obtained  tne  hand  positions,  the  joint  variables  are  determined  and  a 
smooth  curve  is  fitted  for  each  joi-.t.  Tnese  trajecto-ies  reduce  the  execut.on 
time  as  the  arm  need  not  be  stopped  at  eacn  intermediate  position. 

The  planned  trajectories  enable  us  tc  calculate  joint  torques  to  exert  a  given 
hand  force.  If  the  joints  are  efficient  tnen  we  can  exert  the  force  at  the  hand 
bu  driving  the  joints  at  tne  calculated  torques.  The  1 ack  of  efficiency  of  a 
joint  caused  by  friction,  leads  to  erro-s  in  the  force  exerted.  In  the  case  of 
ihe  present  hand  the  error  is  o<  the  order  of  ±10  oz.  at  the  hand.  To  reduce 
these  errors  we  would  need  to  sense  joint  torques,  or  forces  at  the  wrist.  If  ue 
sense  forces  at  the  wrist  a  transformation  matrix  uould  be  needed  to  relate 
joint  drive  to  wrist  force  components. 

At  present  complete  arm  actions  are  written  in  a  file.  Each  file  contains  both 
the  trajectory  and  servo  constants  together  with  the  joint  torques  which  exert  a 
hand  force.  It  is  possible  to  sepa-ate  the  components  and  to  save  the 
trajectory  toqn-ther  with  its  servo  constants.  Such  a  trajectory  can  then  be 
used  when  a  simi  lar  motion  is  needed.  I  f  the  arm  were  also  to  exert  a  force 
this  could  be  calculated  and  added  to  tne  existing  trajectory. 

The  third  problem,  how  to  servo  the  arm.  was  solved  by  writing  a  control  program 
capable  of  servo. ng  the  arm  and  performing  the  various  functions.  When  the  arm 
is  not  used  simply  as  a  positioning  device  the  flexibility  of  a  computer  is 
needed  to  modify  the  course  of  execution  depending  on  many  conditions.  The 
arm's  positional  accuracy  is  ±3.1  inches  and  its  repeatability  is  -3*03  inches. 
The  primary  limitations  on  accuracy  are  the  A/D  converter  and  hnk  stiffness.  If 
the  system  could  make  accurate  differential  motions  ue  could  bring  the  arm  into 
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the  vicinity  of  the  object  and  then  use  either  vision  or  touch  to  detect  the 
differential  errors  and  make  the  appropriate  corrections.  Although  the  present 
system  can  calculate  differential  changes  all  the  joints  must  be  servoed  to 
accomplish  the  change  and  there  is  no  improvement  beyond  the  limits  of  arm 
repeatability.  If  some  precise  form  of  differential  motion  were  possible  then 
the  effective  accuracy  of  the  arm  would  be  increased. 

The  basic  decision  to  move  the  arm  along  trajectories  and  the  subsequent 
decision  to  divide  the  task  into  planning  and  execution  have  facilitated  the 
development  of  both  parts  of  the  work.  For  instance,  without  trajectories  we 
could  not  predict  the  gravity  loading  terms,  nor  could  we  control  the  approach 
to  the  support  plane.  By  writing  two  programs,  one  for  planning  and  one  for 
execution,  it  is  possible  to  optimize  both  separately.  Ue  were  able  to  write 
the  planning  program  in  a  high  level  language  and  to  execute  it  under  time 
sharing,  as  there  are  no  real  time  constraints.  The  execution  program  is  written 
in  assembly  language  and  is  suitable  for  execution  on  a  small  computer. 


8.2  SUGGESTIONS  FOR  FUTURE  UORK 

There  are  two  main  areas  for  future  work,  world  modeling  and  arm  control.  In 
world  modeling  the  determination  of  arm  collisions  with  other  objects  and  the 
subsequent  trajectory  modification  need  to  be  programmed.  The  class  of  objects 
that  can  be  represented  needs  to  be  extended,  together  with  the  hand’s  ability 
to  mainpulate  them. 

In  the  area  of  arm  control  we  need  to  improve  the  sensory  ability  of  the  arm  in 
the  form  of  touch  sensors  and  force  sensing  at  the  wrist.  This  would  make  the 
arm  more  sensitive  to  its  environment,  and  able  to  perform  in  a  more  intelligent 
manner.  With  the  present  system  it  should  be  possible  to  weigh  objects  by 
measuring  the  joint  torques  and  inferring  the  weight.  Ue  could  also  investigate 
the  degrees  of  freedom  of  an  unknown  object  by  exerting  forces  on  it  and 
examining  the  resultant  motion.  There  are  many  tasks  of  this  nature  that  could 
be  performed  with  the  existing  system.  The  use  of  tools  i6  an  important  area  of 
work  to  be  investigated.  Many  tasks  would  be  simplified  by  the  use  of  two 
hands,  one  to  hold  the  work  piece  and  the  other  to  perform  some  operation  on  it, 
Ue  are  currently  installing  a  second  arm  similar  to  the  one  described  here  and 
hope  to  investigate  some  of  these  problems. 


APPENDIX 


A.  1  HARDUARE  DESCRIPTION 

The  arm  is  shown  in  Figure  2.1  [Scheinman]  and  we  will  give  its  essential 
features  here.  The  hand,  a  simple  parallel  jaw  hand  of  10  cm  opening,  can  be 
positioned  anywhere  in  a  work  space  of  1  meter  radiu6  about  the  shoulder  and 
with  any  orientation.  It  is  provided  with  elementary  "switch"  type  touch 
sensors. 

The  arm,  designed  to  work  with  Skg  loads,  is  powered  by  electric  motors; 
harmonic  drive  gear  reductions  are  employed  on  most  joints.  These  reductions  are 
efficient  which  means  that  the  arm  can  react  to  external  forces  (see  Subsection 
6.3).  and  thus  the  hand  can  follow  an  externally  constrained  motion.  Brakes 
are  provided  to  hold  the  arm  in  position  when  it  is  stopped,  so  that  the  arm 
need  not  be  continuously  servoed. 

The  power  to  weight  ratio  is  high,  resulting  in  a  very  high  performance  arm.  It 
is  possible  to  move  the  arm  1S0  degrees  at  the  shoulder  in  little  over  a  second 
including  stopping  and  starting  time.  The  absolute  accuracy  of  the  arm  is  of 
the  order  of  ±0.1  inches  and  its  repeatabi I i ty  ±0.03  inches.  Point  to  point 
servoing  usually  takes  in  the  order  of  1  to  2  seconds. 

Joint  angles  are  measured  by  integral  potentiometers  and  are  read  into  the 
computer  by  a  12  bit  A/D  converter.  In  the  case  of  joint  6  where  continuous 
motion  is  possible  two  wipers  are  provided  on  a  common  element.  Uhenever  the 
current  wiper  is  within  l/8th  of  the  end  of  scale  the  other  wiper  is  read.  As 
wipers  are  interchanged  an  offset  is  added  when  appropriate.  This  continuous 
motion  is  used  in  such  tasks  as  screwing  in  screws. 

Control  of  the  arm  is  by  means  of  a  voltage  pulse  widtn  modulated  signal;  the 
polarity  and  duration  are  set  by  the  computer.  If  the  arm  is  not  addressed  bu 
the  computer  once  every  23  msec,  a  hardware  interlock  autcmat i ca I  I y  stops  the 
motors  and  puts  on  the  brakes.  This  must  be  "unlocked"  by  the  computer  before 
the  arm  can  be  run  again. 


"•Tr 
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A. 2  SAIL 

SAIL  [SwmehartJ  is  an  extended  form  of  ALGOL  with  LEAP  [Feldman  S9I  added  to 
it.  LEAP  provides  the  ITEM  and  DATUil  constructs  which  we  use  to  represent  the 
models.  Bodies  and  parts  of  them  (faces,  vertices,  edges)  are  represented  by 
items,  a  data  type  which  is  treated  simply  as  a  name.  The  main  use  of  items  is 
that  they  may  be  associated  together  in  the  following  manner: 

Attribute  of  Object  is  Value  [Eq.  9.1] 

Where  Attribute.  Object  and  Value  are  items. 

To  associate  three  items  the  "MAKE"  construct  is  used. 

f^E  AaOsV  [Eq.  9.23 

Where  the  "®"  stands  for  "of"  and  the  stands  for  "is". 

To  delete  such  an  association  the  "ERASE"  construct  is  used. 

ERASE  AeOaV  [Eq,  3,3] 


There  exists  a  mechanism  for  searching  the  store  of  associations  in  an  efficient 
manner,  the  "FOREACH"  construct.  Assume  that  we  had  HADE  the  following 
associations:  : 


FACEsCUBEsFACEl 

FACE»CU3E«FACE2 


Then  the  fol lowing  FOREACH  statement: 

FOREACH  F|  FACE®CU3E»F  DO  <statement>  [Eq.  9.4] 


where  <staicment>  is  an  ALGOL  statement  and  F  is  an  item  variable  and  will  cause 
F  to  be  sequentially  assigned  to  FACE1  and  then  to  FACE2,  the  statement  being 
executed  each  time. 

One  additional  piece  of  data  can  be  associated  with  an  item  known  as  its  DATUn. 
This  is  usually  of  algebraic  type,  for  example  an  array.  In  order  to  refer  to 
this  array  by  name  the  DATUM  construct  is  used,  and  to  refer  to  an  element  of 
the  array  the  subscript  list  is  added. 


DATUH (A) 


[Eq.  9.5] 


Thus  if  ue  represent  a  vertex  of  a  body  by  an  item  e.g.  VERTEX1  then  ue  nay  give 
as  its  datum  the  vector  representing  the  position  of  the  vertex,  uhere 
DATUM (VERTEX1) 113  ucuid  be  for  instance  the  "x"  coordinate. 

A. 3  VECTORS  AND  TRANSFORMATIONS 

Vectors  representing  points  in  space  are  denoted  by  an  under-bar  "V"  and  are 
described  by  four  components: 

I  VU]  | 

I  I 
I  V 123  I 
I  I 
|  V  [3]  | 

I  I 
I  V[4]  | 

IEq.  3.63 

such  that  the  components  of  the  vector  V  along  the  x,y,z  axes  is  given  by: 

X  .  V  [1  ] /V  [4]  IEq.  9.7] 

V  -  V  12]  /V  [4] 

Z  -  V  [3] /VIA] 

Uith  this  scheme  the  null  vector: 

I  2  | 

1  0  | 

I  I 

|  0  | 

I  I 
i  1  I 

u‘*t.  9.8] 

and  vectors  at  inifinity: 

I  VII]  | 

I  I 

|  v  [23  | 

I  I 
I  V  [3]  | 

I  I 
I  0  I 

IEq.  9.93 
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are  representable. 

All  usual  vector  operations  are  defined.  Addition  and  subtraction  where: 

-  -  (  *  1  a  ]  lEq.  9.10] 

R II]  -  AIU/AI4]  ±  BQ]/B [4] 

Rt23  -  A  [2] /A  [4]  ±  B[2]/B  [4] 

R 133  -  A 13] /A  [4]  ±  B  C3]  /B  [4] 

R(4]  -  1.0 


The  dot  product  where: 

(A  .  £)  -  (Ail)*Bm  +  A £23  *B  [2]  +  A(33*B(33)/A[4]*B£43 

(Eq.  9.11] 

The  cross  product  where:  R  -  (A  X  £  ) 

R til  -  a [2] *3 [3]  -  B(23*A [3] 

Rf2]  -  A£33*B£13  -  BI33*A£13 
R£3]  -  AQ]*Bl2]  -  B£13*A(23 
R  £4]  -  A  [4]  #B  [4] 

M  2  2  2  1/2 
Magnitude:  I  A  |  -  (All]  +  A 123  fA  (33  )  /A  £4] 

Fca I e: 

I  AQ]  | 

I  I 

I  A  £23  | 

SA  =  j  | 

I  A  £33  | 

I 

I  A  £4] /s I 

Planes  are  also  represented  by  four  components  as  a  row  matrix 

I  Fill  P  £23  F  (33  F  £43  | 


tEq.  9.133 


£Eq.  9.143 


£Eq.  9.153 


in  this  case  the  first  three  components  represent  the  outward  pointing  normal  of 
the  plane  normalized  to  unity  and  the  fourth  component  represents  the  negative 
directed  distance  to  the  plane  in  the  direction  of  the  normal  from  the  origin 
(see  Figure  9.1).  M 


Figure  9.1 
Plane  Description 


Pm  *  NUJ  CEq.  9.  IB] 

F  [23  -  N  [23 
F  (3]  -  N  [3] 

F  [4]  -  -D 


If  a  plane  is  so  represented,  a  simple  test  exists  to  determine  if  a  point  V 
lies  inside  or  outside  the  plane.  Ue  form  the  product: 

I F!  *  I V  |  -  V[1]*F[1J  +  V  (21  *F  12]  +  V[3]*F[3]  +  VI4]*FI4] 

IEq.  9.17] 

and  depending  on  the  sign  the  point  lies  inside  or  outside  the  plane  If  the 
product  is  zero  then  the  point  lies  on  the  plane. 

Having  represented  points  and  planes  it  remains  to  be  able  to  rotate  and 


-  •*»»(** wttmitumftimii 
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cas.  of  points,*  “'rotation  is  ?ep^«enteaU|;ySabfo2rtb^w"Mt??x"a,^i,'■  ,he 

I  T  | 


|  XX 

yx' 

zx' 

0 

*  1  xy' 

yy' 

zy' 

0 

I  xz' 

yz' 

zz* 

0 

1  0 

0 

0 

1 

IEq.  9.183 

«... .«.«  th, 

i  i  i 
i  i 
i  0  ! 

*  -  I  I 

I  0  I 
I  I 
I  1  I 


f  then  transformed  byj 


has  components: 


IEq.  9.19] 


IEq.  9.20] 


XX 

xy' 

xz' 

1 


in  the  rotated  system,  (see  Figure  9.2) 

In  the  case  of  a  translation  we  have  the  matrix: 


T 


1 

0 

0 

0 


0 

1 

0 

0 


0 

0 

1 

0 


xt 

yt 

zt 

l 


IEq.  9.21] 


IEq.  9.22] 
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Figure  9.2 

Rotated  Coordinate  system 
from  which  it  can  be  seen  that: 

|  T  |  *  |  V  |  tEq*  9*23] 


has  components: 


|  VQJ+xt 

i 

|  V [21  +yt 

I 

1  V[31+zt 

! 

|  V  [4] 


CEq.  9.24] 


of  the  translated  vector. 

To  be  able  to  rotate  and  translate  we  multiply  the  transformation  matrices 
together: 


|  T  |  -  |  Tt|  *  |  Tr| 


lEq.  9.25] 
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xx' 

yx' 

yy' 

zx' 

xt 

xy' 

zy‘ 

yt 

xz' 

yz' 

zz1 

zt 

0 

0 

0 

1 

CEq.  9.263 


These  are  the  only 
capable  of  many 
perspective. 


transformations  that  wt  will 
more  transformat  ions  such 


perform  although  the  system  is 
a6  scaling,  stretching  and 


In  the  case  of  planes  we  require 
transforniat ions  such  that  if: 


Equation  9.17 


to  hold 


under  these 


and: 


I  P  I  *  I  V  j  «  |  F‘  |  *  j  V'  | 


CEq.  9.273 


I  V'  |  .  |  T  |  *  j  v  | 

Then  if  we  substitute  for  |  V  |  in  Equation  9.27  we  obtain 

I  F  I  *  I  V  |  -  |  F*  |  *  |  T  |  *  |  y  | 


and  thus: 


or: 


F'  I  *  I  F  j  *  |  T  | 


T  -1  T 

P'  I  -  I  T  j  *  |  F  | 


CEq.  9.283 


CEq.  9.293 


CEq.  9.303 


CEq.  9.313 


t/ansLe^  pre-mgl tiplying  by  the  inverse  transfer. 
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